From f33273d763fdfc0126908d87e6732ba4369d20f6 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Sun, 6 Oct 2024 23:03:12 +0200 Subject: [PATCH 01/13] [69] robo4j-core logger update --- pom.xml | 2 +- robo4j-core/pom.xml | 9 + .../main/java/com/robo4j/RoboApplication.java | 175 ++-- .../com/robo4j/RoboApplicationException.java | 7 +- .../src/main/java/com/robo4j/RoboBuilder.java | 903 ++++++++-------- .../src/main/java/com/robo4j/RoboSystem.java | 985 +++++++++--------- .../com/robo4j/logging/SimpleLoggingUtil.java | 53 +- .../java/com/robo4j/net/ContextEmitter.java | 162 ++- .../net/DefaultLookupServiceBuilder.java | 14 +- .../com/robo4j/net/HearbeatMessageCodec.java | 252 ++--- .../com/robo4j/net/LookupServiceImpl.java | 283 +++-- .../java/com/robo4j/net/MessageClient.java | 337 +++--- .../java/com/robo4j/net/MessageServer.java | 329 +++--- .../com/robo4j/net/ReferenceDescriptor.java | 49 +- .../robo4j/net/ServerRemoteRoboContext.java | 309 +++--- robo4j-core/src/main/java/module-info.java | 1 + .../java/com/robo4j/CounterUnitTests.java | 86 +- 17 files changed, 1957 insertions(+), 1999 deletions(-) diff --git a/pom.xml b/pom.xml index 2526880d..874a319b 100644 --- a/pom.xml +++ b/pom.xml @@ -125,7 +125,7 @@ 3.6.0 3.6.0 2.7.0-SNAPSHOT - 2.0.13 + 2.0.16 1.6.7 diff --git a/robo4j-core/pom.xml b/robo4j-core/pom.xml index 05935389..5ff61f48 100644 --- a/robo4j-core/pom.xml +++ b/robo4j-core/pom.xml @@ -31,6 +31,15 @@ + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine diff --git a/robo4j-core/src/main/java/com/robo4j/RoboApplication.java b/robo4j-core/src/main/java/com/robo4j/RoboApplication.java index 9795ad20..5c8d139d 100644 --- a/robo4j-core/src/main/java/com/robo4j/RoboApplication.java +++ b/robo4j-core/src/main/java/com/robo4j/RoboApplication.java @@ -16,9 +16,10 @@ */ package com.robo4j; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.scheduler.RoboThreadFactory; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.io.InputStream; @@ -37,91 +38,91 @@ * @author Miroslav Wengner (@miragemiko) */ public final class RoboApplication { - - - private static final class ShutdownThread extends Thread { - - private final CountDownLatch latch; - - private ShutdownThread(CountDownLatch latch) { - this.latch = latch; - } - - @Override - public void run() { - latch.countDown(); - } - } - - private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(1, - new RoboThreadFactory(new ThreadGroup("Robo4J-Launcher"), "Robo4J-App-", false)); - private static final CountDownLatch appLatch = new CountDownLatch(1); - - - public RoboApplication() { - } - - /** - * the method is called by standalone robo launcher. - * - * @param context - * robo context - */ - public void launch(RoboContext context) { - // Create a new Launcher thread and then wait for that thread to finish - Runtime.getRuntime().addShutdownHook(new ShutdownThread(appLatch)); - try { - // TODO : review, exception runtime? - Thread daemon = new Thread(() -> { - try { - System.in.read(); - appLatch.countDown(); - } catch (IOException e) { - e.printStackTrace(); - } - - }); - daemon.setName("Robo4J-Launcher-listener"); - daemon.setDaemon(true); - daemon.start(); - context.start(); - - String logo = getBanner(Thread.currentThread().getContextClassLoader()); - SimpleLoggingUtil.info(getClass(), logo); - SimpleLoggingUtil.info(RoboApplication.class, SystemUtil.printStateReport(context)); - SimpleLoggingUtil.info(RoboApplication.class, "Press ..."); - appLatch.await(); - SimpleLoggingUtil.info(RoboApplication.class,"Going down..."); - context.shutdown(); - SimpleLoggingUtil.info(RoboApplication.class,"Bye!"); - } catch (InterruptedException e) { - throw new RoboApplicationException("unexpected", e); - } - } - - public void launchWithExit(RoboContext context, long delay, TimeUnit timeUnit) { - executor.schedule(() -> Runtime.getRuntime().exit(0), delay, timeUnit); - launch(context); - - } - - public void launchNoExit(RoboContext context, long delay, TimeUnit timeUnit) { - executor.schedule(appLatch::countDown, delay, timeUnit); - launch(context); - } - - private String getBanner(ClassLoader classLoader){ - final InputStream is = classLoader.getResourceAsStream("banner.txt"); - final byte[] logoBytes; - try { - logoBytes = is == null ? new byte[0] : is.readAllBytes(); - } catch (IOException e) { - throw new IllegalStateException("not allowed"); - } - - return new StringBuilder().append(BREAK).append(DELIMITER_HORIZONTAL) - .append(new String(logoBytes)).append(BREAK).append(DELIMITER_HORIZONTAL) - .toString(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboApplication.class); + + private static final class ShutdownThread extends Thread { + + private final CountDownLatch latch; + + private ShutdownThread(CountDownLatch latch) { + this.latch = latch; + } + + @Override + public void run() { + latch.countDown(); + } + } + + private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(1, + new RoboThreadFactory(new ThreadGroup("Robo4J-Launcher"), "Robo4J-App-", false)); + private static final CountDownLatch appLatch = new CountDownLatch(1); + + + public RoboApplication() { + } + + /** + * the method is called by standalone robo launcher. + * + * @param context robo context + */ + public void launch(RoboContext context) { + // Create a new Launcher thread and then wait for that thread to finish + Runtime.getRuntime().addShutdownHook(new ShutdownThread(appLatch)); + try { + // TODO : review, exception runtime? + Thread daemon = new Thread(() -> { + try { + System.in.read(); + appLatch.countDown(); + } catch (IOException e) { + LOGGER.error("launch", e); + } + + }); + daemon.setName("Robo4J-Launcher-listener"); + daemon.setDaemon(true); + daemon.start(); + context.start(); + + String logo = getBanner(Thread.currentThread().getContextClassLoader()); + LOGGER.info(logo); + LOGGER.info(SystemUtil.printStateReport(context)); + LOGGER.info("Press ..."); + // TODO : introduce timeout + appLatch.await(); + LOGGER.info("Going down..."); + context.shutdown(); + LOGGER.info("Bye!"); + } catch (InterruptedException e) { + throw new RoboApplicationException("unexpected", e); + } + } + + public void launchWithExit(RoboContext context, long delay, TimeUnit timeUnit) { + executor.schedule(() -> Runtime.getRuntime().exit(0), delay, timeUnit); + launch(context); + + } + + public void launchNoExit(RoboContext context, long delay, TimeUnit timeUnit) { + executor.schedule(appLatch::countDown, delay, timeUnit); + launch(context); + } + + private String getBanner(ClassLoader classLoader) { + final InputStream is = classLoader.getResourceAsStream("banner.txt"); + final byte[] logoBytes; + try { + logoBytes = is == null ? new byte[0] : is.readAllBytes(); + } catch (IOException e) { + throw new IllegalStateException("not allowed"); + } + + return new StringBuilder().append(BREAK).append(DELIMITER_HORIZONTAL) + .append(new String(logoBytes)).append(BREAK).append(DELIMITER_HORIZONTAL) + .toString(); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/RoboApplicationException.java b/robo4j-core/src/main/java/com/robo4j/RoboApplicationException.java index 74f743d0..f6ae2df0 100644 --- a/robo4j-core/src/main/java/com/robo4j/RoboApplicationException.java +++ b/robo4j-core/src/main/java/com/robo4j/RoboApplicationException.java @@ -16,6 +16,8 @@ */ package com.robo4j; +import java.io.Serial; + /** * RoboApplicationException {@link RoboApplication} * @@ -23,9 +25,10 @@ * @author Miroslav Wengner (@miragemiko) */ public class RoboApplicationException extends RuntimeException { - private static final long serialVersionUID = 1L; + @Serial + private static final long serialVersionUID = 1L; - public RoboApplicationException(String message, Throwable cause) { + public RoboApplicationException(String message, Throwable cause) { super(message, cause); } } diff --git a/robo4j-core/src/main/java/com/robo4j/RoboBuilder.java b/robo4j-core/src/main/java/com/robo4j/RoboBuilder.java index 8d2b03bc..178e9007 100644 --- a/robo4j-core/src/main/java/com/robo4j/RoboBuilder.java +++ b/robo4j-core/src/main/java/com/robo4j/RoboBuilder.java @@ -20,9 +20,10 @@ import com.robo4j.configuration.ConfigurationFactory; import com.robo4j.configuration.ConfigurationFactoryException; import com.robo4j.configuration.XmlConfigurationFactory; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.net.LookupServiceProvider; import com.robo4j.util.StringConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import org.xml.sax.Attributes; import org.xml.sax.SAXException; import org.xml.sax.helpers.DefaultHandler; @@ -40,471 +41,445 @@ /** * Builds a RoboSystem from various different sources. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class RoboBuilder { - /** - * Configuration key for the maximum thread size for the scheduler thread - * pool. - */ - public static final String KEY_SCHEDULER_POOL_SIZE = "poolSizeScheduler"; - /** - * Configuration key for the maximum thread size for the worker thread pool. - */ - public static final String KEY_WORKER_POOL_SIZE = "poolSizeWorker"; - /** - * Configuration key for the maximum thread size for the worker thread pool. - */ - public static final String KEY_BLOCKING_POOL_SIZE = "poolSizeBlocking"; - /** - * Configuration key for the child configuration for the message server. - */ - public static final String KEY_CONFIGURATION_SERVER = "com.robo4j.messageServer"; - /** - * Configuration key for the child configuration for the auto discovery. - */ - public static final String KEY_CONFIGURATION_EMITTER = "com.robo4j.discovery"; - /** - * Configuration key for the auto discovery metadata service. - */ - public static final String KEY_CONFIGURATION_EMITTER_METADATA = "com.robo4j.discovery.metadata"; - - private final Set> units = new HashSet<>(); - private final RoboSystem system; - - /** - * Class responsible for the XML parsing. Using SAX to keep down resource - * requirements. - */ - private class RoboXMLHandler extends DefaultHandler { - private static final String ELEMENT_ROBO_UNIT = "roboUnit"; - private String currentId = StringConstants.EMPTY; - private String currentClassName = StringConstants.EMPTY; - private String currentConfiguration = StringConstants.EMPTY; - private String lastElement = StringConstants.EMPTY; - private boolean configState = false; - private boolean inSystemElement = false; - - @Override - public void startElement(String uri, String localName, String qName, Attributes attributes) throws SAXException { - switch (qName) { - case ELEMENT_ROBO_UNIT: - currentId = attributes.getValue("id"); - break; - case SystemXMLHandler.ELEMENT_SYSTEM: - inSystemElement = true; - break; - case XmlConfigurationFactory.ELEMENT_CONFIG: - if (!configState && !inSystemElement) { - currentConfiguration = StringConstants.EMPTY; - configState = true; - break; - } - } - lastElement = qName; - if (configState) { - currentConfiguration += String.format("<%s %s>", qName, toString(attributes)); - } - } - - @Override - public void endElement(String uri, String localName, String qName) throws SAXException { - if (qName.equals(SystemXMLHandler.ELEMENT_SYSTEM)) { - inSystemElement = false; - } else if (qName.equals(ELEMENT_ROBO_UNIT)) { - if (!verifyUnit()) { - clearCurrentVariables(); - } else { - try { - SimpleLoggingUtil.debug(getClass(), "Loading " + currentClassName.trim() + " id=" + currentId); - @SuppressWarnings("unchecked") - Class> roboUnitClass = (Class>) Thread.currentThread().getContextClassLoader() - .loadClass(currentClassName.trim()); - Configuration config = currentConfiguration.trim().equals(StringConstants.EMPTY) ? null - : XmlConfigurationFactory.fromXml(currentConfiguration); - internalAddUnit(instantiateAndInitialize(roboUnitClass, currentId.trim(), config)); - } catch (Exception e) { - throw new SAXException("Failed to parse robo unit", e); - } - clearCurrentVariables(); - } - configState = false; - } - if (configState) { - currentConfiguration += String.format("", qName); - } - } - - @Override - public void characters(char[] ch, int start, int length) throws SAXException { - if (configState) { - currentConfiguration += toString(ch, start, length); - } - // NOTE(Marcus/Jan 22, 2017): Seems these can be called repeatedly - // for a single text() node. - switch (lastElement) { - case XmlConfigurationFactory.ELEMENT_CONFIG: - currentConfiguration += toString(ch, start, length); - break; - case "class": - currentClassName += toString(ch, start, length); - break; - default: - break; - } - } - - private Object toString(Attributes attributes) { - return String.format("%s=\"%s\" %s=\"%s\" %s=\"%s\" %s=\"%s\"", XmlConfigurationFactory.ATTRIBUTE_NAME, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_NAME), XmlConfigurationFactory.ATTRIBUTE_TYPE, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_TYPE), XmlConfigurationFactory.ATTRIBUTE_PATH, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_PATH), XmlConfigurationFactory.ATTRIBUTE_METHOD, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_METHOD)); - } - - private void clearCurrentVariables() { - currentId = StringConstants.EMPTY; - currentClassName = StringConstants.EMPTY; - currentConfiguration = null; - currentConfiguration = StringConstants.EMPTY; - } - - private boolean verifyUnit() { - if (currentId.isEmpty()) { - SimpleLoggingUtil.error(getClass(), "Error parsing unit, no ID"); - return false; - } else if (currentClassName.isEmpty()) { - SimpleLoggingUtil.error(getClass(), "Error parsing unit, no class name for " + currentId); - return false; - } - return true; - } - - private String toString(char[] data, int offset, int count) { - return String.valueOf(data, offset, count); - } - } - - /** - * Separate handler for reading the system part. - */ - private static class SystemXMLHandler extends DefaultHandler { - private static final String ELEMENT_SYSTEM = "roboSystem"; - private String currentId = StringConstants.EMPTY; - private String currentConfiguration = StringConstants.EMPTY; - private String lastElement = StringConstants.EMPTY; - private boolean configState = false; - private RoboSystem system; - - @Override - public void startElement(String uri, String localName, String qName, Attributes attributes) throws SAXException { - switch (qName) { - case ELEMENT_SYSTEM: - currentId = attributes.getValue("id"); - break; - case XmlConfigurationFactory.ELEMENT_CONFIG: - if (!configState) { - currentConfiguration = StringConstants.EMPTY; - configState = true; - break; - } - } - lastElement = qName; - if (configState) { - currentConfiguration += String.format("<%s %s>", qName, toString(attributes)); - } - } - - @Override - public void endElement(String uri, String localName, String qName) throws SAXException { - if (qName.equals(ELEMENT_SYSTEM)) { - SimpleLoggingUtil.debug(getClass(), "Loading system id=" + currentId); - Configuration config; - try { - // TODO : review config = null - config = currentConfiguration.trim().isEmpty() ? null : XmlConfigurationFactory.fromXml(currentConfiguration); - if (currentId == null) { - system = new RoboSystem(config); - } else { - system = new RoboSystem(currentId, config); - } - } catch (ConfigurationFactoryException e) { - SimpleLoggingUtil.error(getClass(), "Error parsing system", e); - } - configState = false; - } - - if (configState) { - currentConfiguration += String.format("", qName); - } - } - - @Override - public void characters(char[] ch, int start, int length) throws SAXException { - if (configState) { - currentConfiguration += toString(ch, start, length); - } - // NOTE(Marcus/Jan 22, 2017): Seems these can be called repeatedly - // for a single text() node. - switch (lastElement) { - case XmlConfigurationFactory.ELEMENT_CONFIG: - currentConfiguration += toString(ch, start, length); - break; - default: - break; - } - } - - private Object toString(Attributes attributes) { - return String.format("%s=\"%s\" %s=\"%s\" %s=\"%s\" %s=\"%s\"", XmlConfigurationFactory.ATTRIBUTE_NAME, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_NAME), XmlConfigurationFactory.ATTRIBUTE_TYPE, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_TYPE), XmlConfigurationFactory.ATTRIBUTE_PATH, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_PATH), XmlConfigurationFactory.ATTRIBUTE_METHOD, - attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_METHOD)); - } - - private String toString(char[] data, int offset, int count) { - return String.valueOf(data, offset, count); - } - - public RoboSystem createSystem() { - return system == null ? new RoboSystem() : system; - } - } - - /** - * Constructor. - */ - public RoboBuilder() { - system = new RoboSystem(); - } - - /** - * Use this builder constructor to configure the system. - * - * @param systemConfig - * initial system configuration - */ - public RoboBuilder(Configuration systemConfig) { - system = new RoboSystem(systemConfig); - } - - /** - * Use this builder constructor to configure the system. - * - * @param uid - * a manually configured unique identifier for the system - * @param systemConfig - * initial system configuration - */ - public RoboBuilder(String uid, Configuration systemConfig) { - system = new RoboSystem(uid, systemConfig); - } - - /** - * Use this builder constructor to configure the RoboSystem. - * - * @param systemConfig - * the configuration settings for the system. Note that this is - * separate from the unit definitions today. - * @throws RoboBuilderException - * possible exception - */ - public RoboBuilder(InputStream systemConfig) throws RoboBuilderException { - system = readSystemFromXML(systemConfig); - } - - /** - * Adds a Robo4J unit to the builder. - * - * @param unit - * an initialized Robo4J unit. Must be initialized (see - * {@link RoboUnit#initialize(Configuration)}) before added. - * - * @return the builder. - * @throws RoboBuilderException - * possible exception - * - * @see RoboUnit#initialize(Configuration) - */ - public RoboBuilder add(RoboUnit unit) throws RoboBuilderException { - internalAddUnit(unit); - return this; - } - - /** - * Adds a collection of Robo4J units to the builder. - * - * @param units - * a collection of units to add. - * - * @return the builder. - * @throws RoboBuilderException - * possible exception - * - * @see RoboUnit#initialize(Configuration) - */ - public RoboBuilder addAll(Collection> units) throws RoboBuilderException { - for (RoboUnit unit : units) { - add(unit); - } - return this; - } - - /** - * Adds an array of Robo4J units to the builder. - * - * @param units - * a collection of units to add. - * - * @return the builder. - * @throws RoboBuilderException - * possible exception - * - * @see RoboUnit#initialize(Configuration) - */ - public RoboBuilder addAll(RoboUnit... units) throws RoboBuilderException { - for (RoboUnit unit : units) { - add(unit); - } - return this; - } - - /** - * Instantiates a RoboUnit from the provided class using the provided id and - * adds it. The instance will be initialized using an empty configuration. - * - * @param clazz - * the class to use. - * @param id - * the id for the instantiated RoboUnit. - * @return the builder, for chaining. - * @throws RoboBuilderException - * if the creation or adding of the unit failed. - */ - public RoboBuilder add(Class> clazz, String id) throws RoboBuilderException { - internalAddUnit(instantiateAndInitialize(clazz, id, ConfigurationFactory.createEmptyConfiguration())); - return this; - } - - /** - * Instantiates a RoboUnit from the provided class using the provided id, - * initializes it and adds it. - * - * @param clazz - * the class to use. - * @param configuration - * RoboUnit configuration - * @param id - * the id for the instantiated RoboUnit. - * @return the builder, for chaining. - * @throws RoboBuilderException - * if the creation or adding of the unit failed. - */ - public RoboBuilder add(Class> clazz, Configuration configuration, String id) throws RoboBuilderException { - internalAddUnit(instantiateAndInitialize(clazz, id, configuration)); - return this; - } - - /** - * Returns the built {@link RoboContext}. This should be the final method - * called on the builder. - * - * @return the RoboContext. - */ - public RoboContext build() { - system.addUnits(units); - LookupServiceProvider.registerLocalContext(system); - system.setState(LifecycleState.INITIALIZED); - return system; - } - - /** - * Returns the context being built. This should only be used so that units - * can be instantiated with more control outside the builder. Note that you - * should then add the instances to the builder through add. - * - * @return the context being built. - */ - public RoboContext getContext() { - return system; - } - - /** - * Will load all the units from the definitions found in the XML file and - * add them to the builder. - * - * @param inputStream - * the xml file containing the definitions. - * - * @return the builder. - * @throws RoboBuilderException - * possible exception - */ - public RoboBuilder add(InputStream inputStream) throws RoboBuilderException { - try { - SAXParser saxParser = SAXParserFactory.newInstance().newSAXParser(); - saxParser.parse(inputStream, new RoboXMLHandler()); - } catch (SAXException | IOException | ParserConfigurationException e) { - throw new RoboBuilderException("Could not initialize from xml", e); - } - return this; - } - - // FIXME(Marcus/Jan 22, 2017): Implement. - // public RoboBuilder add(ClassLoader loader) throws RoboBuilderException { - // throw new UnsupportedOperationException("Not yet supported"); - // } - - // FIXME(Marcus/Jan 22, 2017): Implement. - // public RoboBuilder add(ClassLoader loader, String match) throws - // RoboBuilderException { - // throw new UnsupportedOperationException("Not yet supported"); - // } - - private void internalAddUnit(RoboUnit unit) throws RoboBuilderException { - if (unit == null) { - throw new RoboBuilderException("Cannot add the null unit! Skipping"); - } else if (units.contains(unit)) { - throw new RoboBuilderException( - "Only one unit with the id " + unit.getId() + " can be active at a time. Skipping " + unit.toString()); - } - units.add(unit); - } - - private RoboUnit instantiateRoboUnit(Class> clazz, String id) throws RoboBuilderException { - try { - Constructor> constructor = clazz.getConstructor(RoboContext.class, String.class); - return constructor.newInstance(system, id); - } catch (NoSuchMethodException | SecurityException | InstantiationException | IllegalAccessException | IllegalArgumentException - | InvocationTargetException e) { - throw new RoboBuilderException("Could not instantiate robo unit.", e); - } - } - - private RoboUnit instantiateAndInitialize(Class> clazz, String id, Configuration configuration) - throws RoboBuilderException { - RoboUnit unit = instantiateRoboUnit(clazz, id); - if (configuration != null) { - try { - unit.initialize(configuration); - } catch (Exception e) { - throw new RoboBuilderException("Error initializing RoboUnit", e); - } - } - return unit; - } - - private RoboSystem readSystemFromXML(InputStream stream) throws RoboBuilderException { - try { - SystemXMLHandler handler = new SystemXMLHandler(); - SAXParser saxParser = SAXParserFactory.newInstance().newSAXParser(); - saxParser.parse(stream, handler); - return handler.createSystem(); - } catch (SAXException | IOException | ParserConfigurationException e) { - throw new RoboBuilderException("Could not initialize system from xml", e); - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboBuilder.class); + /** + * Configuration key for the maximum thread size for the scheduler thread + * pool. + */ + public static final String KEY_SCHEDULER_POOL_SIZE = "poolSizeScheduler"; + /** + * Configuration key for the maximum thread size for the worker thread pool. + */ + public static final String KEY_WORKER_POOL_SIZE = "poolSizeWorker"; + /** + * Configuration key for the maximum thread size for the worker thread pool. + */ + public static final String KEY_BLOCKING_POOL_SIZE = "poolSizeBlocking"; + /** + * Configuration key for the child configuration for the message server. + */ + public static final String KEY_CONFIGURATION_SERVER = "com.robo4j.messageServer"; + /** + * Configuration key for the child configuration for the auto discovery. + */ + public static final String KEY_CONFIGURATION_EMITTER = "com.robo4j.discovery"; + /** + * Configuration key for the auto discovery metadata service. + */ + public static final String KEY_CONFIGURATION_EMITTER_METADATA = "com.robo4j.discovery.metadata"; + + private final Set> units = new HashSet<>(); + private final RoboSystem system; + + /** + * Class responsible for the XML parsing. Using SAX to keep down resource + * requirements. + */ + private class RoboXMLHandler extends DefaultHandler { + private static final String ELEMENT_ROBO_UNIT = "roboUnit"; + private String currentId = StringConstants.EMPTY; + private String currentClassName = StringConstants.EMPTY; + private String currentConfiguration = StringConstants.EMPTY; + private String lastElement = StringConstants.EMPTY; + private boolean configState = false; + private boolean inSystemElement = false; + + @Override + public void startElement(String uri, String localName, String qName, Attributes attributes) throws SAXException { + switch (qName) { + case ELEMENT_ROBO_UNIT: + currentId = attributes.getValue("id"); + break; + case SystemXMLHandler.ELEMENT_SYSTEM: + inSystemElement = true; + break; + case XmlConfigurationFactory.ELEMENT_CONFIG: + if (!configState && !inSystemElement) { + currentConfiguration = StringConstants.EMPTY; + configState = true; + break; + } + } + lastElement = qName; + if (configState) { + currentConfiguration += String.format("<%s %s>", qName, toString(attributes)); + } + } + + @Override + public void endElement(String uri, String localName, String qName) throws SAXException { + if (qName.equals(SystemXMLHandler.ELEMENT_SYSTEM)) { + inSystemElement = false; + } else if (qName.equals(ELEMENT_ROBO_UNIT)) { + if (!verifyUnit()) { + clearCurrentVariables(); + } else { + try { + LOGGER.debug("Loading {} id={}", currentClassName.trim(), currentId); + @SuppressWarnings("unchecked") + Class> roboUnitClass = (Class>) Thread.currentThread().getContextClassLoader() + .loadClass(currentClassName.trim()); + Configuration config = currentConfiguration.trim().equals(StringConstants.EMPTY) ? null + : XmlConfigurationFactory.fromXml(currentConfiguration); + internalAddUnit(instantiateAndInitialize(roboUnitClass, currentId.trim(), config)); + } catch (Exception e) { + throw new SAXException("Failed to parse robo unit", e); + } + clearCurrentVariables(); + } + configState = false; + } + if (configState) { + currentConfiguration += String.format("", qName); + } + } + + @Override + public void characters(char[] ch, int start, int length) throws SAXException { + if (configState) { + currentConfiguration += toString(ch, start, length); + } + // NOTE(Marcus/Jan 22, 2017): Seems these can be called repeatedly + // for a single text() node. + switch (lastElement) { + case XmlConfigurationFactory.ELEMENT_CONFIG: + currentConfiguration += toString(ch, start, length); + break; + case "class": + currentClassName += toString(ch, start, length); + break; + default: + break; + } + } + + private Object toString(Attributes attributes) { + return String.format("%s=\"%s\" %s=\"%s\" %s=\"%s\" %s=\"%s\"", XmlConfigurationFactory.ATTRIBUTE_NAME, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_NAME), XmlConfigurationFactory.ATTRIBUTE_TYPE, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_TYPE), XmlConfigurationFactory.ATTRIBUTE_PATH, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_PATH), XmlConfigurationFactory.ATTRIBUTE_METHOD, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_METHOD)); + } + + private void clearCurrentVariables() { + currentId = StringConstants.EMPTY; + currentClassName = StringConstants.EMPTY; + currentConfiguration = StringConstants.EMPTY; + } + + private boolean verifyUnit() { + if (currentId.isEmpty()) { + LOGGER.error("Error parsing unit, no ID"); + return false; + } else if (currentClassName.isEmpty()) { + LOGGER.error("Error parsing unit, no class name for {}", currentId); + return false; + } + return true; + } + + private String toString(char[] data, int offset, int count) { + return String.valueOf(data, offset, count); + } + } + + /** + * Separate handler for reading the system part. + */ + private static class SystemXMLHandler extends DefaultHandler { + private static final String ELEMENT_SYSTEM = "roboSystem"; + private String currentId = StringConstants.EMPTY; + private String currentConfiguration = StringConstants.EMPTY; + private String lastElement = StringConstants.EMPTY; + private boolean configState = false; + private RoboSystem system; + + @Override + public void startElement(String uri, String localName, String qName, Attributes attributes) throws SAXException { + switch (qName) { + case ELEMENT_SYSTEM: + currentId = attributes.getValue("id"); + break; + case XmlConfigurationFactory.ELEMENT_CONFIG: + if (!configState) { + currentConfiguration = StringConstants.EMPTY; + configState = true; + break; + } + } + lastElement = qName; + if (configState) { + currentConfiguration += String.format("<%s %s>", qName, toString(attributes)); + } + } + + @Override + public void endElement(String uri, String localName, String qName) throws SAXException { + if (qName.equals(ELEMENT_SYSTEM)) { + LOGGER.debug("Loading system id={}", currentId); + Configuration config; + try { + // TODO : review config = null + config = currentConfiguration.trim().isEmpty() ? null : XmlConfigurationFactory.fromXml(currentConfiguration); + if (currentId == null) { + system = new RoboSystem(config); + } else { + system = new RoboSystem(currentId, config); + } + } catch (ConfigurationFactoryException e) { + LOGGER.error("Error parsing system", e); + } + configState = false; + } + + if (configState) { + currentConfiguration += String.format("", qName); + } + } + + @Override + public void characters(char[] ch, int start, int length) throws SAXException { + if (configState) { + currentConfiguration += toString(ch, start, length); + } + // NOTE(Marcus/Jan 22, 2017): Seems these can be called repeatedly + // for a single text() node. + switch (lastElement) { + case XmlConfigurationFactory.ELEMENT_CONFIG: + currentConfiguration += toString(ch, start, length); + break; + default: + break; + } + } + + private Object toString(Attributes attributes) { + return String.format("%s=\"%s\" %s=\"%s\" %s=\"%s\" %s=\"%s\"", XmlConfigurationFactory.ATTRIBUTE_NAME, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_NAME), XmlConfigurationFactory.ATTRIBUTE_TYPE, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_TYPE), XmlConfigurationFactory.ATTRIBUTE_PATH, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_PATH), XmlConfigurationFactory.ATTRIBUTE_METHOD, + attributes.getValue(XmlConfigurationFactory.ATTRIBUTE_METHOD)); + } + + private String toString(char[] data, int offset, int count) { + return String.valueOf(data, offset, count); + } + + public RoboSystem createSystem() { + return system == null ? new RoboSystem() : system; + } + } + + /** + * Constructor. + */ + public RoboBuilder() { + system = new RoboSystem(); + } + + /** + * Use this builder constructor to configure the system. + * + * @param systemConfig initial system configuration + */ + public RoboBuilder(Configuration systemConfig) { + system = new RoboSystem(systemConfig); + } + + /** + * Use this builder constructor to configure the system. + * + * @param uid a manually configured unique identifier for the system + * @param systemConfig initial system configuration + */ + public RoboBuilder(String uid, Configuration systemConfig) { + system = new RoboSystem(uid, systemConfig); + } + + /** + * Use this builder constructor to configure the RoboSystem. + * + * @param systemConfig the configuration settings for the system. Note that this is + * separate from the unit definitions today. + * @throws RoboBuilderException possible exception + */ + public RoboBuilder(InputStream systemConfig) throws RoboBuilderException { + system = readSystemFromXML(systemConfig); + } + + /** + * Adds a Robo4J unit to the builder. + * + * @param unit an initialized Robo4J unit. Must be initialized (see + * {@link RoboUnit#initialize(Configuration)}) before added. + * @return the builder. + * @throws RoboBuilderException possible exception + * @see RoboUnit#initialize(Configuration) + */ + public RoboBuilder add(RoboUnit unit) throws RoboBuilderException { + internalAddUnit(unit); + return this; + } + + /** + * Adds a collection of Robo4J units to the builder. + * + * @param units a collection of units to add. + * @return the builder. + * @throws RoboBuilderException possible exception + * @see RoboUnit#initialize(Configuration) + */ + public RoboBuilder addAll(Collection> units) throws RoboBuilderException { + for (RoboUnit unit : units) { + add(unit); + } + return this; + } + + /** + * Adds an array of Robo4J units to the builder. + * + * @param units a collection of units to add. + * @return the builder. + * @throws RoboBuilderException possible exception + * @see RoboUnit#initialize(Configuration) + */ + public RoboBuilder addAll(RoboUnit... units) throws RoboBuilderException { + for (RoboUnit unit : units) { + add(unit); + } + return this; + } + + /** + * Instantiates a RoboUnit from the provided class using the provided id and + * adds it. The instance will be initialized using an empty configuration. + * + * @param clazz the class to use. + * @param id the id for the instantiated RoboUnit. + * @return the builder, for chaining. + * @throws RoboBuilderException if the creation or adding of the unit failed. + */ + public RoboBuilder add(Class> clazz, String id) throws RoboBuilderException { + internalAddUnit(instantiateAndInitialize(clazz, id, ConfigurationFactory.createEmptyConfiguration())); + return this; + } + + /** + * Instantiates a RoboUnit from the provided class using the provided id, + * initializes it and adds it. + * + * @param clazz the class to use. + * @param configuration RoboUnit configuration + * @param id the id for the instantiated RoboUnit. + * @return the builder, for chaining. + * @throws RoboBuilderException if the creation or adding of the unit failed. + */ + public RoboBuilder add(Class> clazz, Configuration configuration, String id) throws RoboBuilderException { + internalAddUnit(instantiateAndInitialize(clazz, id, configuration)); + return this; + } + + /** + * Returns the built {@link RoboContext}. This should be the final method + * called on the builder. + * + * @return the RoboContext. + */ + public RoboContext build() { + system.addUnits(units); + LookupServiceProvider.registerLocalContext(system); + system.setState(LifecycleState.INITIALIZED); + return system; + } + + /** + * Returns the context being built. This should only be used so that units + * can be instantiated with more control outside the builder. Note that you + * should then add the instances to the builder through add. + * + * @return the context being built. + */ + public RoboContext getContext() { + return system; + } + + /** + * Will load all the units from the definitions found in the XML file and + * add them to the builder. + * + * @param inputStream the xml file containing the definitions. + * @return the builder. + * @throws RoboBuilderException possible exception + */ + public RoboBuilder add(InputStream inputStream) throws RoboBuilderException { + try { + SAXParser saxParser = SAXParserFactory.newInstance().newSAXParser(); + saxParser.parse(inputStream, new RoboXMLHandler()); + } catch (SAXException | IOException | ParserConfigurationException e) { + throw new RoboBuilderException("Could not initialize from xml", e); + } + return this; + } + + // FIXME(Marcus/Jan 22, 2017): Implement. + // public RoboBuilder add(ClassLoader loader) throws RoboBuilderException { + // throw new UnsupportedOperationException("Not yet supported"); + // } + + // FIXME(Marcus/Jan 22, 2017): Implement. + // public RoboBuilder add(ClassLoader loader, String match) throws + // RoboBuilderException { + // throw new UnsupportedOperationException("Not yet supported"); + // } + + private void internalAddUnit(RoboUnit unit) throws RoboBuilderException { + if (unit == null) { + throw new RoboBuilderException("Cannot add the null unit! Skipping"); + } else if (units.contains(unit)) { + throw new RoboBuilderException( + "Only one unit with the id " + unit.getId() + " can be active at a time. Skipping " + unit.toString()); + } + units.add(unit); + } + + private RoboUnit instantiateRoboUnit(Class> clazz, String id) throws RoboBuilderException { + try { + Constructor> constructor = clazz.getConstructor(RoboContext.class, String.class); + return constructor.newInstance(system, id); + } catch (NoSuchMethodException | SecurityException | InstantiationException | IllegalAccessException | + IllegalArgumentException + | InvocationTargetException e) { + throw new RoboBuilderException("Could not instantiate robo unit.", e); + } + } + + private RoboUnit instantiateAndInitialize(Class> clazz, String id, Configuration configuration) + throws RoboBuilderException { + RoboUnit unit = instantiateRoboUnit(clazz, id); + if (configuration != null) { + try { + unit.initialize(configuration); + } catch (Exception e) { + throw new RoboBuilderException("Error initializing RoboUnit", e); + } + } + return unit; + } + + private RoboSystem readSystemFromXML(InputStream stream) throws RoboBuilderException { + try { + SystemXMLHandler handler = new SystemXMLHandler(); + SAXParser saxParser = SAXParserFactory.newInstance().newSAXParser(); + saxParser.parse(stream, handler); + return handler.createSystem(); + } catch (SAXException | IOException | ParserConfigurationException e) { + throw new RoboBuilderException("Could not initialize system from xml", e); + } + } } diff --git a/robo4j-core/src/main/java/com/robo4j/RoboSystem.java b/robo4j-core/src/main/java/com/robo4j/RoboSystem.java index a0423e7e..aad75d80 100644 --- a/robo4j-core/src/main/java/com/robo4j/RoboSystem.java +++ b/robo4j-core/src/main/java/com/robo4j/RoboSystem.java @@ -18,36 +18,23 @@ import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; -import com.robo4j.logging.SimpleLoggingUtil; -import com.robo4j.net.ContextEmitter; -import com.robo4j.net.MessageCallback; -import com.robo4j.net.MessageServer; -import com.robo4j.net.ReferenceDescriptor; -import com.robo4j.net.RoboContextDescriptor; +import com.robo4j.net.*; import com.robo4j.scheduler.DefaultScheduler; import com.robo4j.scheduler.RoboThreadFactory; import com.robo4j.scheduler.Scheduler; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.io.ObjectStreamException; +import java.io.Serial; import java.io.Serializable; import java.net.SocketException; import java.net.URI; import java.net.UnknownHostException; -import java.util.Collection; -import java.util.EnumSet; -import java.util.HashMap; -import java.util.Map; -import java.util.Set; -import java.util.UUID; -import java.util.WeakHashMap; -import java.util.concurrent.Executors; -import java.util.concurrent.Future; -import java.util.concurrent.LinkedBlockingQueue; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.ThreadPoolExecutor; -import java.util.concurrent.TimeUnit; +import java.util.*; +import java.util.concurrent.*; import java.util.concurrent.atomic.AtomicReference; import java.util.stream.Collectors; @@ -55,487 +42,489 @@ * This is the default implementation for a local {@link RoboContext}. Contains * RoboUnits, a lookup service for references to RoboUnits, and a system level * life cycle. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ final class RoboSystem implements RoboContext { - private static final String NAME_BLOCKING_POOL = "Robo4J Blocking Pool"; - private static final String NAME_WORKER_POOL = "Robo4J Worker Pool"; - private static final int DEFAULT_BLOCKING_POOL_SIZE = 4; - private static final int DEFAULT_WORKER_POOL_SIZE = 2; - private static final int DEFAULT_SCHEDULER_POOL_SIZE = 2; - private static final int KEEP_ALIVE_TIME = 10; - - private static final EnumSet MESSAGE_DELIVERY_CRITERIA = EnumSet.of(LifecycleState.STARTED, LifecycleState.STOPPED, - LifecycleState.STOPPING); - - private final AtomicReference state = new AtomicReference<>(LifecycleState.UNINITIALIZED); - private final Map> units = new HashMap<>(); - private final Map, RoboReference> referenceCache = new WeakHashMap<>(); - - private final Scheduler systemScheduler; - - private final ThreadPoolExecutor workExecutor; - private final LinkedBlockingQueue workQueue = new LinkedBlockingQueue<>(); - - private final ThreadPoolExecutor blockingExecutor; - private final LinkedBlockingQueue blockingQueue = new LinkedBlockingQueue<>(); - - private final String uid; - private final Configuration configuration; - - private final MessageServer messageServer; - private final Configuration emitterConfiguration; - private volatile ScheduledFuture emitterFuture; - - private enum DeliveryPolicy { - SYSTEM, WORK, BLOCKING - } - - private enum ThreadingPolicy { - NORMAL, CRITICAL - } - - private class LocalRoboReference implements RoboReference, Serializable { - private static final long serialVersionUID = 1L; - private final RoboUnit unit; - private final DeliveryPolicy deliveryPolicy; - private final ThreadingPolicy threadingPolicy; - - LocalRoboReference(RoboUnit unit) { - this.unit = unit; - @SuppressWarnings("unchecked") - Class> clazz = (Class>) unit.getClass(); - this.deliveryPolicy = deriveDeliveryPolicy(clazz); - this.threadingPolicy = deriveThreadingPolicy(clazz); - } - - private ThreadingPolicy deriveThreadingPolicy(Class> clazz) { - if (clazz.getAnnotation(CriticalSectionTrait.class) != null) { - return ThreadingPolicy.CRITICAL; - } - return ThreadingPolicy.NORMAL; - } - - private DeliveryPolicy deriveDeliveryPolicy(Class> clazz) { - if (clazz.getAnnotation(WorkTrait.class) != null) { - return DeliveryPolicy.WORK; - } - if (clazz.getAnnotation(BlockingTrait.class) != null) { - return DeliveryPolicy.BLOCKING; - } - return DeliveryPolicy.SYSTEM; - } - - @Override - public String getId() { - return unit.getId(); - } - - @Override - public LifecycleState getState() { - return unit.getState(); - } - - @Override - public Configuration getConfiguration() { - return unit.getConfiguration(); - } - - @Override - public void sendMessage(T message) { - if (MESSAGE_DELIVERY_CRITERIA.contains(getState())) { - switch (threadingPolicy) { - case NORMAL: - deliverOnQueue(message); - break; - case CRITICAL: - synchronized (unit) { - deliverOnQueue(message); - } - break; - default: - throw new IllegalStateException(String.format("not supported policy: %s", threadingPolicy)); - } - } - } - - @Override - public String toString() { - return "LocalReference id: " + unit.getId() + " (system: " + uid + ")"; - } - - private void deliverOnQueue(T message) { - switch (deliveryPolicy) { - case SYSTEM: - systemScheduler.execute(new Messenger(unit, message)); - break; - case WORK: - workExecutor.execute(new Messenger(unit, message)); - break; - case BLOCKING: - blockingExecutor.execute(new Messenger(unit, message)); - break; - default: - SimpleLoggingUtil.error(getClass(), "not supported policy: " + deliveryPolicy); - } - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - return systemScheduler.submit(() -> unit.onGetAttribute(attribute)); - } - - @Override - public Collection> getKnownAttributes() { - return unit.getKnownAttributes(); - } - - @Override - public Future, Object>> getAttributes() { - return systemScheduler.submit(unit::onGetAttributes); - } - - @Override - public Class getMessageType() { - return unit.getMessageType(); - } - - Object writeReplace() throws ObjectStreamException { - return new ReferenceDescriptor(RoboSystem.this.getId(), getId(), getMessageType().getName()); - } - } - - // Protects the executors from problems in the units. - private static class Messenger implements Runnable { - private final RoboUnit unit; - private final T message; - - public Messenger(RoboUnit unit, T message) { - this.unit = unit; - this.message = message; - } - - @Override - public void run() { - try { - unit.onMessage(message); - } catch (Throwable t) { - SimpleLoggingUtil.error(unit.getClass(), "Error processing message", t); - } - } - } - - /** - * Constructor. - */ - RoboSystem() { - this(UUID.randomUUID().toString()); - } - - /** - * Constructor. - */ - RoboSystem(String uid) { - this(uid, DEFAULT_SCHEDULER_POOL_SIZE, DEFAULT_WORKER_POOL_SIZE, DEFAULT_BLOCKING_POOL_SIZE); - } - - /** - * Constructor. - */ - RoboSystem(String uid, Configuration configuration) { - this.uid = uid; - this.configuration = configuration; - int schedulerPoolSize = configuration.getInteger(RoboBuilder.KEY_SCHEDULER_POOL_SIZE, DEFAULT_SCHEDULER_POOL_SIZE); - int workerPoolSize = configuration.getInteger(RoboBuilder.KEY_WORKER_POOL_SIZE, DEFAULT_WORKER_POOL_SIZE); - int blockingPoolSize = configuration.getInteger(RoboBuilder.KEY_BLOCKING_POOL_SIZE, DEFAULT_SCHEDULER_POOL_SIZE); - workExecutor = new ThreadPoolExecutor(workerPoolSize, workerPoolSize, KEEP_ALIVE_TIME, TimeUnit.SECONDS, workQueue, - new RoboThreadFactory(new ThreadGroup(NAME_WORKER_POOL), NAME_WORKER_POOL, true)); - blockingExecutor = new ThreadPoolExecutor(blockingPoolSize, blockingPoolSize, KEEP_ALIVE_TIME, TimeUnit.SECONDS, blockingQueue, - new RoboThreadFactory(new ThreadGroup(NAME_BLOCKING_POOL), NAME_BLOCKING_POOL, true)); - systemScheduler = new DefaultScheduler(this, schedulerPoolSize); - messageServer = initServer(configuration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_SERVER)); - emitterConfiguration = configuration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_EMITTER); - } - - /** - * Constructor. - */ - RoboSystem(Configuration config) { - this(UUID.randomUUID().toString(), config); - } - - /** - * Convenience constructor. - */ - RoboSystem(String uid, int schedulerPoolSize, int workerPoolSize, int blockingPoolSize) { - this(uid, createConfiguration(schedulerPoolSize, workerPoolSize, blockingPoolSize)); - } - - @Override - public Configuration getConfiguration() { - return configuration; - } - - /** - * Adds the specified units to the system. - * - * @param unitSet - * the units to add. - */ - public void addUnits(Set> unitSet) { - if (state.get() != LifecycleState.UNINITIALIZED) { - throw new UnsupportedOperationException("All units must be registered up front for now."); - } - addToMap(unitSet); - } - - /** - * Adds the specified units to the system. - * - * @param units - * the units to add. - */ - public void addUnits(RoboUnit... units) { - if (state.get() != LifecycleState.UNINITIALIZED) { - throw new UnsupportedOperationException("All units must be registered up front for now."); - } - addToMap(units); - } - - @Override - public void start() { - if (state.compareAndSet(LifecycleState.STOPPED, LifecycleState.STARTING)) { - // NOTE(Marcus/Sep 4, 2017): Do we want to support starting a - // stopped system? - startUnits(); - } - if (state.compareAndSet(LifecycleState.INITIALIZED, LifecycleState.STARTING)) { - startUnits(); - } - - // This is only used from testing for now, it should never happen from - // the builder. - if (state.compareAndSet(LifecycleState.UNINITIALIZED, LifecycleState.STARTING)) { - startUnits(); - } - - // If we have a server, start it, then set up emitter - blockingExecutor.execute(new Runnable() { - @Override - public void run() { - if (messageServer != null) { - try { - messageServer.start(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not start the message server. Proceeding without.", e); - } - } - } - }); - final ContextEmitter emitter = initEmitter(emitterConfiguration, getListeningURI(messageServer)); - if (emitter != null) { - emitterFuture = getScheduler().scheduleAtFixedRate(emitter::emit, 0, emitter.getHeartBeatInterval(), TimeUnit.MILLISECONDS); - } - } - - private void startUnits() { - // NOTE(Marcus/Sep 4, 2017): May want to schedule the starts. - for (RoboUnit unit : units.values()) { - unit.setState(LifecycleState.STARTING); - unit.start(); - unit.setState(LifecycleState.STARTED); - } - state.set(LifecycleState.STARTED); - } - - @Override - public void stop() { - // NOTE(Marcus/Sep 4, 2017): We may want schedule the stops in the - // future. - if (emitterFuture != null) { - emitterFuture.cancel(true); - } - if (messageServer != null) { - messageServer.stop(); - } - if (state.compareAndSet(LifecycleState.STARTED, LifecycleState.STOPPING)) { - units.values().forEach(RoboUnit::stop); - } - state.set(LifecycleState.STOPPED); - } - - @Override - public void shutdown() { - stop(); - state.set(LifecycleState.SHUTTING_DOWN); - units.values().forEach((unit) -> unit.setState(LifecycleState.SHUTTING_DOWN)); - - // First shutdown all executors. We don't care at this point, as any - // messages will no longer be delivered. - workExecutor.shutdown(); - blockingExecutor.shutdown(); - - // Then schedule shutdowns on the scheduler threads... - for (RoboUnit unit : units.values()) { - getScheduler().execute(new Runnable() { - @Override - public void run() { - RoboSystem.shutdownUnit(unit); - } - }); - } - - // Then shutdown the system scheduler. Will wait until the termination - // shutdown of the system scheduler (or the timeout). - try { - systemScheduler.shutdown(); - } catch (InterruptedException e) { - SimpleLoggingUtil.error(getClass(), "System scheduler was interrupted when shutting down.", e); - } - state.set(LifecycleState.SHUTDOWN); - } - - @Override - public LifecycleState getState() { - return state.get(); - } - - public void setState(LifecycleState state) { - this.state.set(state); - } - - @Override - public Collection> getUnits() { - return units.values().stream().map(this::getReference).collect(Collectors.toList()); - } - - @Override - public RoboReference getReference(String id) { - @SuppressWarnings("unchecked") - RoboUnit roboUnit = (RoboUnit) units.get(id); - if (roboUnit == null) { - return null; - } - return getReference(roboUnit); - } - - @Override - public Scheduler getScheduler() { - return systemScheduler; - } - - @Override - public String getId() { - return uid; - } - - /** - * Returns the reference for a specific unit. - * - * @param roboUnit - * the robo unit for which to retrieve a reference. - * @return the {@link RoboReference} to the unit. - */ - public RoboReference getReference(RoboUnit roboUnit) { - @SuppressWarnings("unchecked") - RoboReference reference = (RoboReference) referenceCache.get(roboUnit); - if (reference == null) { - reference = createReference(roboUnit); - referenceCache.put(roboUnit, reference); - } - return reference; - } - - @Override - public String toString() { - return "RoboSystem id: " + uid + " unit count: " + units.size(); - } - - private RoboReference createReference(RoboUnit roboUnit) { - return new LocalRoboReference<>(roboUnit); - } - - private void addToMap(Set> unitSet) { - unitSet.forEach(unit -> units.put(unit.getId(), unit)); - } - - private void addToMap(RoboUnit... unitArray) { - // NOTE(Marcus/Aug 9, 2017): Do not streamify... - for (RoboUnit unit : unitArray) { - units.put(unit.getId(), unit); - } - } - - private static void shutdownUnit(RoboUnit unit) { - // NOTE(Marcus/Aug 11, 2017): Should really be scheduled and done in - // parallel. - unit.shutdown(); - unit.setState(LifecycleState.SHUTDOWN); - } - - private static Configuration createConfiguration(int schedulerPoolSize, int workerPoolSize, int blockingPoolSize) { - return new ConfigurationBuilder().addInteger(RoboBuilder.KEY_SCHEDULER_POOL_SIZE, schedulerPoolSize) - .addInteger(RoboBuilder.KEY_WORKER_POOL_SIZE, workerPoolSize).addInteger(RoboBuilder.KEY_BLOCKING_POOL_SIZE, blockingPoolSize).build(); - } - - private MessageServer initServer(Configuration serverConfiguration) { - if (serverConfiguration != null) { - return new MessageServer(new MessageCallback() { - @Override - public void handleMessage(String sourceUuid, String id, Object message) { - getReference(id).sendMessage(message); - } - }, serverConfiguration); - } else { - return null; - } - } - - private ContextEmitter initEmitter(Configuration emitterConfiguration, URI uri) { - if (messageServer != null && uri != null) { - if (emitterConfiguration.getBoolean(ContextEmitter.KEY_ENABLED, Boolean.FALSE)) { - try { - return new ContextEmitter(createDescriptor(uri, emitterConfiguration), emitterConfiguration); - } catch (SocketException | UnknownHostException e) { - SimpleLoggingUtil.error(getClass(), "Could not initialize autodiscovery emitter. Proceeding without!", e); - } - } else { - SimpleLoggingUtil.info(getClass(), "Context emitter disabled in settings. Proceeding without!"); - } - } else { - SimpleLoggingUtil.info(getClass(), "Will not use context emitter. Server was: " + messageServer + " uri was: " + uri); - } - return null; - } - - private RoboContextDescriptor createDescriptor(URI uri, Configuration emitterConfiguration) { - int heartbeatInterval = emitterConfiguration.getInteger(ContextEmitter.KEY_HEARTBEAT_INTERVAL, - ContextEmitter.DEFAULT_HEARTBEAT_INTERVAL); - Map metadata = toStringMap(emitterConfiguration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_EMITTER_METADATA)); - metadata.put(RoboContextDescriptor.KEY_URI, uri.toString()); - return new RoboContextDescriptor(getId(), heartbeatInterval, metadata); - } - - private Map toStringMap(Configuration configuration) { - Map map = new HashMap<>(); - for (String name : configuration.getValueNames()) { - map.put(name, configuration.getString(name, null)); - } - return map; - } - - private static URI getListeningURI(MessageServer server) { - if (server != null) { - for (int i = 0; i < 5; i++) { - URI uri = server.getListeningURI(); - if (uri != null) { - return uri; - } - SystemUtil.sleep(100); - } - } - return null; - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboSystem.class); + private static final String NAME_BLOCKING_POOL = "Robo4J Blocking Pool"; + private static final String NAME_WORKER_POOL = "Robo4J Worker Pool"; + private static final int DEFAULT_BLOCKING_POOL_SIZE = 4; + private static final int DEFAULT_WORKER_POOL_SIZE = 2; + private static final int DEFAULT_SCHEDULER_POOL_SIZE = 2; + private static final int KEEP_ALIVE_TIME = 10; + + private static final EnumSet MESSAGE_DELIVERY_CRITERIA = EnumSet.of(LifecycleState.STARTED, LifecycleState.STOPPED, + LifecycleState.STOPPING); + + private final AtomicReference state = new AtomicReference<>(LifecycleState.UNINITIALIZED); + private final Map> units = new HashMap<>(); + private final Map, RoboReference> referenceCache = new WeakHashMap<>(); + + private final Scheduler systemScheduler; + + private final ThreadPoolExecutor workExecutor; + private final LinkedBlockingQueue workQueue = new LinkedBlockingQueue<>(); + + private final ThreadPoolExecutor blockingExecutor; + private final LinkedBlockingQueue blockingQueue = new LinkedBlockingQueue<>(); + + private final String uid; + private final Configuration configuration; + + private final MessageServer messageServer; + private final Configuration emitterConfiguration; + private volatile ScheduledFuture emitterFuture; + + private enum DeliveryPolicy { + SYSTEM, WORK, BLOCKING + } + + private enum ThreadingPolicy { + NORMAL, CRITICAL + } + + private class LocalRoboReference implements RoboReference, Serializable { + @Serial + private static final long serialVersionUID = 1L; + private static final Logger LOGGER_LOCAL = LoggerFactory.getLogger(LocalRoboReference.class); + private final RoboUnit unit; + private final DeliveryPolicy deliveryPolicy; + private final ThreadingPolicy threadingPolicy; + + LocalRoboReference(RoboUnit unit) { + this.unit = unit; + @SuppressWarnings("unchecked") + Class> clazz = (Class>) unit.getClass(); + this.deliveryPolicy = deriveDeliveryPolicy(clazz); + this.threadingPolicy = deriveThreadingPolicy(clazz); + } + + private ThreadingPolicy deriveThreadingPolicy(Class> clazz) { + if (clazz.getAnnotation(CriticalSectionTrait.class) != null) { + return ThreadingPolicy.CRITICAL; + } + return ThreadingPolicy.NORMAL; + } + + private DeliveryPolicy deriveDeliveryPolicy(Class> clazz) { + if (clazz.getAnnotation(WorkTrait.class) != null) { + return DeliveryPolicy.WORK; + } + if (clazz.getAnnotation(BlockingTrait.class) != null) { + return DeliveryPolicy.BLOCKING; + } + return DeliveryPolicy.SYSTEM; + } + + @Override + public String getId() { + return unit.getId(); + } + + @Override + public LifecycleState getState() { + return unit.getState(); + } + + @Override + public Configuration getConfiguration() { + return unit.getConfiguration(); + } + + @Override + public void sendMessage(T message) { + if (MESSAGE_DELIVERY_CRITERIA.contains(getState())) { + switch (threadingPolicy) { + case NORMAL: + deliverOnQueue(message); + break; + case CRITICAL: + synchronized (unit) { + deliverOnQueue(message); + } + break; + default: + throw new IllegalStateException(String.format("not supported policy: %s", threadingPolicy)); + } + } + } + + @Override + public String toString() { + return "LocalReference id: " + unit.getId() + " (system: " + uid + ")"; + } + + private void deliverOnQueue(T message) { + switch (deliveryPolicy) { + case SYSTEM: + systemScheduler.execute(new Messenger(unit, message)); + break; + case WORK: + workExecutor.execute(new Messenger(unit, message)); + break; + case BLOCKING: + blockingExecutor.execute(new Messenger(unit, message)); + break; + default: + LOGGER_LOCAL.error("not supported policy: {}", deliveryPolicy); + } + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + return systemScheduler.submit(() -> unit.onGetAttribute(attribute)); + } + + @Override + public Collection> getKnownAttributes() { + return unit.getKnownAttributes(); + } + + @Override + public Future, Object>> getAttributes() { + return systemScheduler.submit(unit::onGetAttributes); + } + + @Override + public Class getMessageType() { + return unit.getMessageType(); + } + + @Serial + Object writeReplace() throws ObjectStreamException { + return new ReferenceDescriptor(RoboSystem.this.getId(), getId(), getMessageType().getName()); + } + } + + // Protects the executors from problems in the units. + private static class Messenger implements Runnable { + private static final Logger LOGGER_MESSENGER = LoggerFactory.getLogger(Messenger.class); + private final RoboUnit unit; + private final T message; + + public Messenger(RoboUnit unit, T message) { + this.unit = unit; + this.message = message; + } + + @Override + public void run() { + try { + unit.onMessage(message); + } catch (Throwable t) { + LOGGER_MESSENGER.error("Error processing message, unit:{}", unit.getId(), t); + } + } + } + + /** + * Constructor. + */ + RoboSystem() { + this(UUID.randomUUID().toString()); + } + + /** + * Constructor. + */ + RoboSystem(String uid) { + this(uid, DEFAULT_SCHEDULER_POOL_SIZE, DEFAULT_WORKER_POOL_SIZE, DEFAULT_BLOCKING_POOL_SIZE); + } + + /** + * Constructor. + */ + RoboSystem(String uid, Configuration configuration) { + this.uid = uid; + this.configuration = configuration; + int schedulerPoolSize = configuration.getInteger(RoboBuilder.KEY_SCHEDULER_POOL_SIZE, DEFAULT_SCHEDULER_POOL_SIZE); + int workerPoolSize = configuration.getInteger(RoboBuilder.KEY_WORKER_POOL_SIZE, DEFAULT_WORKER_POOL_SIZE); + int blockingPoolSize = configuration.getInteger(RoboBuilder.KEY_BLOCKING_POOL_SIZE, DEFAULT_SCHEDULER_POOL_SIZE); + workExecutor = new ThreadPoolExecutor(workerPoolSize, workerPoolSize, KEEP_ALIVE_TIME, TimeUnit.SECONDS, workQueue, + new RoboThreadFactory(new ThreadGroup(NAME_WORKER_POOL), NAME_WORKER_POOL, true)); + blockingExecutor = new ThreadPoolExecutor(blockingPoolSize, blockingPoolSize, KEEP_ALIVE_TIME, TimeUnit.SECONDS, blockingQueue, + new RoboThreadFactory(new ThreadGroup(NAME_BLOCKING_POOL), NAME_BLOCKING_POOL, true)); + systemScheduler = new DefaultScheduler(this, schedulerPoolSize); + messageServer = initServer(configuration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_SERVER)); + emitterConfiguration = configuration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_EMITTER); + } + + /** + * Constructor. + */ + RoboSystem(Configuration config) { + this(UUID.randomUUID().toString(), config); + } + + /** + * Convenience constructor. + */ + RoboSystem(String uid, int schedulerPoolSize, int workerPoolSize, int blockingPoolSize) { + this(uid, createConfiguration(schedulerPoolSize, workerPoolSize, blockingPoolSize)); + } + + @Override + public Configuration getConfiguration() { + return configuration; + } + + /** + * Adds the specified units to the system. + * + * @param unitSet the units to add. + */ + public void addUnits(Set> unitSet) { + if (state.get() != LifecycleState.UNINITIALIZED) { + throw new UnsupportedOperationException("All units must be registered up front for now."); + } + addToMap(unitSet); + } + + /** + * Adds the specified units to the system. + * + * @param units the units to add. + */ + public void addUnits(RoboUnit... units) { + if (state.get() != LifecycleState.UNINITIALIZED) { + throw new UnsupportedOperationException("All units must be registered up front for now."); + } + addToMap(units); + } + + @Override + public void start() { + if (state.compareAndSet(LifecycleState.STOPPED, LifecycleState.STARTING)) { + // NOTE(Marcus/Sep 4, 2017): Do we want to support starting a + // stopped system? + startUnits(); + } + if (state.compareAndSet(LifecycleState.INITIALIZED, LifecycleState.STARTING)) { + startUnits(); + } + + // This is only used from testing for now, it should never happen from + // the builder. + if (state.compareAndSet(LifecycleState.UNINITIALIZED, LifecycleState.STARTING)) { + startUnits(); + } + + // If we have a server, start it, then set up emitter + blockingExecutor.execute(new Runnable() { + @Override + public void run() { + if (messageServer != null) { + try { + messageServer.start(); + } catch (IOException e) { + LOGGER.error("Could not start the message server. Proceeding without.", e); + } + } + } + }); + final ContextEmitter emitter = initEmitter(emitterConfiguration, getListeningURI(messageServer)); + if (emitter != null) { + emitterFuture = getScheduler().scheduleAtFixedRate(emitter::emit, 0, emitter.getHeartBeatInterval(), TimeUnit.MILLISECONDS); + } + } + + private void startUnits() { + // NOTE(Marcus/Sep 4, 2017): May want to schedule the starts. + for (RoboUnit unit : units.values()) { + unit.setState(LifecycleState.STARTING); + unit.start(); + unit.setState(LifecycleState.STARTED); + } + state.set(LifecycleState.STARTED); + } + + @Override + public void stop() { + // NOTE(Marcus/Sep 4, 2017): We may want schedule the stops in the + // future. + if (emitterFuture != null) { + emitterFuture.cancel(true); + } + if (messageServer != null) { + messageServer.stop(); + } + if (state.compareAndSet(LifecycleState.STARTED, LifecycleState.STOPPING)) { + units.values().forEach(RoboUnit::stop); + } + state.set(LifecycleState.STOPPED); + } + + @Override + public void shutdown() { + stop(); + state.set(LifecycleState.SHUTTING_DOWN); + units.values().forEach((unit) -> unit.setState(LifecycleState.SHUTTING_DOWN)); + + // First shutdown all executors. We don't care at this point, as any + // messages will no longer be delivered. + workExecutor.shutdown(); + blockingExecutor.shutdown(); + + // Then schedule shutdowns on the scheduler threads... + for (RoboUnit unit : units.values()) { + getScheduler().execute(new Runnable() { + @Override + public void run() { + RoboSystem.shutdownUnit(unit); + } + }); + } + + // Then shutdown the system scheduler. Will wait until the termination + // shutdown of the system scheduler (or the timeout). + try { + systemScheduler.shutdown(); + } catch (InterruptedException e) { + LOGGER.error("System scheduler was interrupted when shutting down.", e); + } + state.set(LifecycleState.SHUTDOWN); + } + + @Override + public LifecycleState getState() { + return state.get(); + } + + public void setState(LifecycleState state) { + this.state.set(state); + } + + @Override + public Collection> getUnits() { + return units.values().stream().map(this::getReference).collect(Collectors.toList()); + } + + @Override + public RoboReference getReference(String id) { + @SuppressWarnings("unchecked") + RoboUnit roboUnit = (RoboUnit) units.get(id); + if (roboUnit == null) { + return null; + } + return getReference(roboUnit); + } + + @Override + public Scheduler getScheduler() { + return systemScheduler; + } + + @Override + public String getId() { + return uid; + } + + /** + * Returns the reference for a specific unit. + * + * @param roboUnit the robo unit for which to retrieve a reference. + * @return the {@link RoboReference} to the unit. + */ + public RoboReference getReference(RoboUnit roboUnit) { + @SuppressWarnings("unchecked") + RoboReference reference = (RoboReference) referenceCache.get(roboUnit); + if (reference == null) { + reference = createReference(roboUnit); + referenceCache.put(roboUnit, reference); + } + return reference; + } + + @Override + public String toString() { + return "RoboSystem id: " + uid + " unit count: " + units.size(); + } + + private RoboReference createReference(RoboUnit roboUnit) { + return new LocalRoboReference<>(roboUnit); + } + + private void addToMap(Set> unitSet) { + unitSet.forEach(unit -> units.put(unit.getId(), unit)); + } + + private void addToMap(RoboUnit... unitArray) { + // NOTE(Marcus/Aug 9, 2017): Do not streamify... + for (RoboUnit unit : unitArray) { + units.put(unit.getId(), unit); + } + } + + private static void shutdownUnit(RoboUnit unit) { + // NOTE(Marcus/Aug 11, 2017): Should really be scheduled and done in + // parallel. + unit.shutdown(); + unit.setState(LifecycleState.SHUTDOWN); + } + + private static Configuration createConfiguration(int schedulerPoolSize, int workerPoolSize, int blockingPoolSize) { + return new ConfigurationBuilder().addInteger(RoboBuilder.KEY_SCHEDULER_POOL_SIZE, schedulerPoolSize) + .addInteger(RoboBuilder.KEY_WORKER_POOL_SIZE, workerPoolSize).addInteger(RoboBuilder.KEY_BLOCKING_POOL_SIZE, blockingPoolSize).build(); + } + + private MessageServer initServer(Configuration serverConfiguration) { + if (serverConfiguration != null) { + return new MessageServer(new MessageCallback() { + @Override + public void handleMessage(String sourceUuid, String id, Object message) { + getReference(id).sendMessage(message); + } + }, serverConfiguration); + } else { + return null; + } + } + + private ContextEmitter initEmitter(Configuration emitterConfiguration, URI uri) { + if (messageServer != null && uri != null) { + if (emitterConfiguration.getBoolean(ContextEmitter.KEY_ENABLED, Boolean.FALSE)) { + try { + return new ContextEmitter(createDescriptor(uri, emitterConfiguration), emitterConfiguration); + } catch (SocketException | UnknownHostException e) { + LOGGER.error("Could not initialize autodiscovery emitter. Proceeding without!", e); + } + } else { + LOGGER.info("Context emitter disabled in settings. Proceeding without!"); + } + } else { + LOGGER.info("Will not use context emitter. Server was: " + messageServer + " uri was: " + uri); + } + return null; + } + + private RoboContextDescriptor createDescriptor(URI uri, Configuration emitterConfiguration) { + int heartbeatInterval = emitterConfiguration.getInteger(ContextEmitter.KEY_HEARTBEAT_INTERVAL, + ContextEmitter.DEFAULT_HEARTBEAT_INTERVAL); + Map metadata = toStringMap(emitterConfiguration.getChildConfiguration(RoboBuilder.KEY_CONFIGURATION_EMITTER_METADATA)); + metadata.put(RoboContextDescriptor.KEY_URI, uri.toString()); + return new RoboContextDescriptor(getId(), heartbeatInterval, metadata); + } + + private Map toStringMap(Configuration configuration) { + Map map = new HashMap<>(); + for (String name : configuration.getValueNames()) { + map.put(name, configuration.getString(name, null)); + } + return map; + } + + private static URI getListeningURI(MessageServer server) { + if (server != null) { + for (int i = 0; i < 5; i++) { + URI uri = server.getListeningURI(); + if (uri != null) { + return uri; + } + SystemUtil.sleep(100); + } + } + return null; + } } diff --git a/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java b/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java index 31272785..e54abee4 100644 --- a/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java +++ b/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java @@ -28,39 +28,40 @@ * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ +@Deprecated public final class SimpleLoggingUtil { - // TODO(Marcus/Sep 6, 2017): Decide if we want to kill this... - public static void print(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message); - } + // TODO(Marcus/Sep 6, 2017): Decide if we want to kill this... + public static void print(Class clazz, String message) { + Logger.getLogger(clazz.getName()).log(Level.INFO, message); + } - public static void debug(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.FINE, message); - } + public static void debug(Class clazz, String message) { + Logger.getLogger(clazz.getName()).log(Level.FINE, message); + } - public static void debug(Class clazz, String... message) { - debug(clazz, clazz.getSimpleName() + " : " - + Stream.of(message).reduce(StringConstants.EMPTY, (l, r) -> l.concat(StringConstants.SPACE).concat(r))); - } + public static void debug(Class clazz, String... message) { + debug(clazz, clazz.getSimpleName() + " : " + + Stream.of(message).reduce(StringConstants.EMPTY, (l, r) -> l.concat(StringConstants.SPACE).concat(r))); + } - public static void debug(Class clazz, String message, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); - } + public static void debug(Class clazz, String message, Throwable e) { + Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); + } - public static void info(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message); - } + public static void info(Class clazz, String message) { + Logger.getLogger(clazz.getName()).log(Level.INFO, message); + } - public static void info(Class clazz, String message, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); - } + public static void info(Class clazz, String message, Throwable e) { + Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); + } - public static void error(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.SEVERE, message); - } + public static void error(Class clazz, String message) { + Logger.getLogger(clazz.getName()).log(Level.SEVERE, message); + } - public static void error(Class clazz, String string, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.SEVERE, string, e); - } + public static void error(Class clazz, String string, Throwable e) { + Logger.getLogger(clazz.getName()).log(Level.SEVERE, string, e); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/ContextEmitter.java b/robo4j-core/src/main/java/com/robo4j/net/ContextEmitter.java index 3ae2537c..cd29e44b 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/ContextEmitter.java +++ b/robo4j-core/src/main/java/com/robo4j/net/ContextEmitter.java @@ -18,111 +18,103 @@ import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.net.DatagramPacket; -import java.net.DatagramSocket; -import java.net.InetAddress; -import java.net.SocketException; -import java.net.UnknownHostException; +import java.net.*; /** * This class is used by the {@link RoboContext} to make it discoverable. This * can be configured in the settings for the {@link RoboContext}. * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class ContextEmitter { - /** - * Defaults to {@link #DEFAULT_HEARTBEAT_INTERVAL} - */ - public static final String KEY_HEARTBEAT_INTERVAL = "heartBeatInterval"; + private static final Logger LOGGER = LoggerFactory.getLogger(ContextEmitter.class); + + /** + * Defaults to {@link #DEFAULT_HEARTBEAT_INTERVAL} + */ + public static final String KEY_HEARTBEAT_INTERVAL = "heartBeatInterval"; - /** - * Defaults to {@link LookupServiceProvider#DEFAULT_MULTICAST_ADDRESS} - */ - public static final String KEY_MULTICAST_ADDRESS = "multicastAddress"; + /** + * Defaults to {@link LookupServiceProvider#DEFAULT_MULTICAST_ADDRESS} + */ + public static final String KEY_MULTICAST_ADDRESS = "multicastAddress"; - /** - * Defaults to {@link LookupServiceProvider#DEFAULT_PORT} - */ - public static final String KEY_PORT = "port"; + /** + * Defaults to {@link LookupServiceProvider#DEFAULT_PORT} + */ + public static final String KEY_PORT = "port"; - /** - * Defaults to false. - */ - public static final String KEY_ENABLED = "enabled"; + /** + * Defaults to false. + */ + public static final String KEY_ENABLED = "enabled"; - /** - * The default heartbeat interval. - */ - public static final Integer DEFAULT_HEARTBEAT_INTERVAL = 1000; + /** + * The default heartbeat interval. + */ + public static final Integer DEFAULT_HEARTBEAT_INTERVAL = 1000; - private final InetAddress multicastAddress; - private final int port; - private final int heartBeatInterval; - private final DatagramSocket socket; - private final byte[] message; + private final InetAddress multicastAddress; + private final int port; + private final int heartBeatInterval; + private final DatagramSocket socket; + private final byte[] message; - /** - * Constructor. - * - * @param entry - * the information to emit. - * @param multicastAddress - * the address to emit to. - * @param port - * the port. - * @param heartBeatInterval - * the heart beat interval - * @throws SocketException - * possible exception - */ - public ContextEmitter(RoboContextDescriptor entry, InetAddress multicastAddress, int port, int heartBeatInterval) - throws SocketException { - this.multicastAddress = multicastAddress; - this.port = port; - this.heartBeatInterval = heartBeatInterval; - socket = new DatagramSocket(); - message = HearbeatMessageCodec.encode(entry); - } + /** + * Constructor. + * + * @param entry the information to emit. + * @param multicastAddress the address to emit to. + * @param port the port. + * @param heartBeatInterval the heart beat interval + * @throws SocketException possible exception + */ + public ContextEmitter(RoboContextDescriptor entry, InetAddress multicastAddress, int port, int heartBeatInterval) + throws SocketException { + this.multicastAddress = multicastAddress; + this.port = port; + this.heartBeatInterval = heartBeatInterval; + socket = new DatagramSocket(); + message = HearbeatMessageCodec.encode(entry); + } - public ContextEmitter(RoboContextDescriptor entry, Configuration emitterConfiguration) - throws SocketException, UnknownHostException { - this(entry, - InetAddress.getByName(emitterConfiguration.getString(KEY_MULTICAST_ADDRESS, - LookupServiceProvider.DEFAULT_MULTICAST_ADDRESS)), - emitterConfiguration.getInteger(KEY_PORT, LookupServiceProvider.DEFAULT_PORT), - emitterConfiguration.getInteger(KEY_HEARTBEAT_INTERVAL, DEFAULT_HEARTBEAT_INTERVAL)); - } + public ContextEmitter(RoboContextDescriptor entry, Configuration emitterConfiguration) + throws SocketException, UnknownHostException { + this(entry, + InetAddress.getByName(emitterConfiguration.getString(KEY_MULTICAST_ADDRESS, + LookupServiceProvider.DEFAULT_MULTICAST_ADDRESS)), + emitterConfiguration.getInteger(KEY_PORT, LookupServiceProvider.DEFAULT_PORT), + emitterConfiguration.getInteger(KEY_HEARTBEAT_INTERVAL, DEFAULT_HEARTBEAT_INTERVAL)); + } - /** - * Emits a context heartbeat message. Will log any problems. - */ - public void emit() { - try { - emitWithException(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to emit heartbeat message", e); - } - } + /** + * Emits a context heartbeat message. Will log any problems. + */ + public void emit() { + try { + emitWithException(); + } catch (IOException e) { + LOGGER.error("Failed to emit heartbeat message", e); + } + } - /** - * Emits a context heartbeat message. Will throw an exception on trouble. - * - * @throws IOException - * exception - */ - public void emitWithException() throws IOException { - DatagramPacket packet = new DatagramPacket(message, message.length, multicastAddress, port); - socket.send(packet); - } + /** + * Emits a context heartbeat message. Will throw an exception on trouble. + * + * @throws IOException exception + */ + public void emitWithException() throws IOException { + DatagramPacket packet = new DatagramPacket(message, message.length, multicastAddress, port); + socket.send(packet); + } - public int getHeartBeatInterval() { - return heartBeatInterval; - } + public int getHeartBeatInterval() { + return heartBeatInterval; + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/DefaultLookupServiceBuilder.java b/robo4j-core/src/main/java/com/robo4j/net/DefaultLookupServiceBuilder.java index 590def87..6f993227 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/DefaultLookupServiceBuilder.java +++ b/robo4j-core/src/main/java/com/robo4j/net/DefaultLookupServiceBuilder.java @@ -16,7 +16,8 @@ */ package com.robo4j.net; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.net.SocketException; import java.net.UnknownHostException; @@ -28,18 +29,18 @@ * @author Miroslav Wengner (@miragemiko) */ public class DefaultLookupServiceBuilder { - + private static final Logger LOGGER = LoggerFactory.getLogger(DefaultLookupServiceBuilder.class); private String address; private Integer port; private Float missedHeartbeatsBeforeRemoval; private LocalLookupServiceImpl localContexts; - private DefaultLookupServiceBuilder(){ + private DefaultLookupServiceBuilder() { } - public static DefaultLookupServiceBuilder Build(){ + public static DefaultLookupServiceBuilder Build() { return new DefaultLookupServiceBuilder(); } @@ -63,12 +64,11 @@ public DefaultLookupServiceBuilder setLocalContexts(LocalLookupServiceImpl local return this; } - public LookupService build(){ + public LookupService build() { try { return new LookupServiceImpl(address, port, missedHeartbeatsBeforeRemoval, localContexts); } catch (SocketException | UnknownHostException e) { - SimpleLoggingUtil.error(LookupServiceProvider.class, - "Failed to set up LookupService! No multicast route? Will use null provider...", e); + LOGGER.error("Failed to set up LookupService! No multicast route? Will use null provider...", e); return new NullLookupService(); } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/HearbeatMessageCodec.java b/robo4j-core/src/main/java/com/robo4j/net/HearbeatMessageCodec.java index b521958d..f0d3a018 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/HearbeatMessageCodec.java +++ b/robo4j-core/src/main/java/com/robo4j/net/HearbeatMessageCodec.java @@ -16,7 +16,8 @@ */ package com.robo4j.net; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.HashMap; import java.util.Map; @@ -24,133 +25,132 @@ /** * Used to encode and decode heartbeat messages, with a minimum of allocation. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class HearbeatMessageCodec { - private static byte PROTOCOL_VERSION = 0; - private static int PROTOCOL_VERSION_BYTE_LENGTH = 1; - private static int PACKAGE_LENGTH_BYTE_LENGTH = 4; - private static int ID_LENGTH_BYTE_LENGTH = 2; - private static int HEART_BEAT_PERIOD_BYTE_LENGTH = 4; - private static int MAX_U2 = 65535; - private static int MAX_U1 = 255; - - private static byte[] MAGIC = new byte[] { (byte) 0xC0, (byte) 0xFF }; - - public static byte[] encode(RoboContextDescriptor entry) { - byte[] message = new byte[calculateEncodedLength(entry)]; - message[0] = MAGIC[0]; - message[1] = MAGIC[1]; - message[2] = PROTOCOL_VERSION; - encodeU4(message, 3, message.length); - encodeU4(message, 7, entry.getHeartBeatInterval()); - String id = crop(MAX_U2, entry.getId(), "id"); - encodeU2(message, 11, id.getBytes().length); - System.arraycopy(entry.getId().getBytes(), 0, message, 13, id.getBytes().length); - encodeMetadata(13 + id.getBytes().length, message, entry.getMetadata()); - return message; - } - - public static RoboContextDescriptor decode(byte[] message) { - if (!isHeartBeatMessage(message)) { - throw new IllegalArgumentException("Invalid message! Must start with the proper magic."); - } - int version = getSupportedVersion(message); - if (version != PROTOCOL_VERSION) { - throw new IllegalArgumentException("Unsupported version " + version + "! Must be " + PROTOCOL_VERSION); - } - int messageLength = decodeS4(message, 3); - int heartBeatInterval = decodeS4(message, 7); - int idLength = decodeU2(message, 11); - String id = new String(message, 13, idLength); - return new RoboContextDescriptor(id, heartBeatInterval, decodeMetadata(message, 13 + idLength, messageLength)); - } - - public static boolean isSupportedVersion(byte[] data) { - return data[2] == PROTOCOL_VERSION; - } - - public static boolean isHeartBeatMessage(byte[] message) { - return message[0] == MAGIC[0] && message[1] == MAGIC[1]; - } - - public static String parseId(byte[] message) { - int idLength = decodeU2(message, 11); - return new String(message, 13, idLength); - } - - private static Map decodeMetadata(byte[] message, int offset, int messageLength) { - Map metadata = new HashMap<>(); - while (offset < messageLength) { - int keyLength = message[offset++]; - String key = new String(message, offset, keyLength); - offset += keyLength; - int valueLength = decodeU2(message, offset); - offset += 2; - String value = new String(message, offset, valueLength); - metadata.put(key, value); - offset += valueLength; - } - return metadata; - } - - private static int decodeU2(byte[] message, int offset) { - return ((0xFF & message[offset]) << 8) | (0xFF & message[offset + 1]); - } - - private static int decodeS4(byte[] message, int offset) { - return ((0xFF & message[offset]) << 24) | ((0xFF & message[offset + 1]) << 16) | ((0xFF & message[offset + 2]) << 8) - | (0xFF & message[offset + 3]); - } - - private static int getSupportedVersion(byte[] message) { - return message[2]; - } - - private static void encodeMetadata(int offset, byte[] message, Map metadata) { - for (Entry entry : metadata.entrySet()) { - String key = crop(MAX_U1, entry.getKey(), "metadata key"); - String value = crop(MAX_U2, entry.getValue(), "metadata value"); - message[offset++] = (byte) key.getBytes().length; - System.arraycopy(key.getBytes(), 0, message, offset, key.getBytes().length); - offset += key.getBytes().length; - encodeU2(message, offset, value.getBytes().length); - offset += 2; - System.arraycopy(value.getBytes(), 0, message, offset, value.getBytes().length); - offset += value.getBytes().length; - } - } - - private static String crop(int maxLength, String string, String entityName) { - if (string.getBytes().length > maxLength) { - SimpleLoggingUtil.error(HearbeatMessageCodec.class, "The string for " + entityName + " was too long for the protocol (max = " - + maxLength + ") and has been cropped. The value started with " + string.substring(0, 20)); - return string.substring(0, maxLength); - } - return string; - } - - private static void encodeU2(byte[] buffer, int offset, int value) { - buffer[offset++] = (byte) ((value & 0x0000FF00) >> 8); - buffer[offset] = (byte) (value & 0x000000FF); - } - - private static void encodeU4(byte[] buffer, int offset, int value) { - buffer[offset++] = (byte) ((value & 0xFF000000) >> 24); - buffer[offset++] = (byte) ((value & 0x00FF0000) >> 16); - buffer[offset++] = (byte) ((value & 0x0000FF00) >> 8); - buffer[offset] = (byte) (value & 0x000000FF); - } - - private static int calculateEncodedLength(RoboContextDescriptor entry) { - return MAGIC.length + PROTOCOL_VERSION_BYTE_LENGTH + PACKAGE_LENGTH_BYTE_LENGTH + HEART_BEAT_PERIOD_BYTE_LENGTH - + ID_LENGTH_BYTE_LENGTH + entry.getId().getBytes().length + calculateMetadataByteLength(entry.getMetadata()); - } - - private static int calculateMetadataByteLength(Map metadata) { - return metadata.entrySet().stream().map(entry -> 1 + Math.min(entry.getKey().getBytes().length, MAX_U1) + 2 - + Math.min(entry.getValue().getBytes().length, MAX_U2)).reduce(0, Integer::sum); - } + private static final Logger LOGGER = LoggerFactory.getLogger(HearbeatMessageCodec.class); + private static final byte PROTOCOL_VERSION = 0; + private static final int PROTOCOL_VERSION_BYTE_LENGTH = 1; + private static final int PACKAGE_LENGTH_BYTE_LENGTH = 4; + private static final int ID_LENGTH_BYTE_LENGTH = 2; + private static final int HEART_BEAT_PERIOD_BYTE_LENGTH = 4; + private static final int MAX_U2 = 65535; + private static final int MAX_U1 = 255; + private static final byte[] MAGIC = new byte[]{(byte) 0xC0, (byte) 0xFF}; + + public static byte[] encode(RoboContextDescriptor entry) { + byte[] message = new byte[calculateEncodedLength(entry)]; + message[0] = MAGIC[0]; + message[1] = MAGIC[1]; + message[2] = PROTOCOL_VERSION; + encodeU4(message, 3, message.length); + encodeU4(message, 7, entry.getHeartBeatInterval()); + String id = crop(MAX_U2, entry.getId(), "id"); + encodeU2(message, 11, id.getBytes().length); + System.arraycopy(entry.getId().getBytes(), 0, message, 13, id.getBytes().length); + encodeMetadata(13 + id.getBytes().length, message, entry.getMetadata()); + return message; + } + + public static RoboContextDescriptor decode(byte[] message) { + if (!isHeartBeatMessage(message)) { + throw new IllegalArgumentException("Invalid message! Must start with the proper magic."); + } + int version = getSupportedVersion(message); + if (version != PROTOCOL_VERSION) { + throw new IllegalArgumentException("Unsupported version " + version + "! Must be " + PROTOCOL_VERSION); + } + int messageLength = decodeS4(message, 3); + int heartBeatInterval = decodeS4(message, 7); + int idLength = decodeU2(message, 11); + String id = new String(message, 13, idLength); + return new RoboContextDescriptor(id, heartBeatInterval, decodeMetadata(message, 13 + idLength, messageLength)); + } + + public static boolean isSupportedVersion(byte[] data) { + return data[2] == PROTOCOL_VERSION; + } + + public static boolean isHeartBeatMessage(byte[] message) { + return message[0] == MAGIC[0] && message[1] == MAGIC[1]; + } + + public static String parseId(byte[] message) { + int idLength = decodeU2(message, 11); + return new String(message, 13, idLength); + } + + private static Map decodeMetadata(byte[] message, int offset, int messageLength) { + Map metadata = new HashMap<>(); + while (offset < messageLength) { + int keyLength = message[offset++]; + String key = new String(message, offset, keyLength); + offset += keyLength; + int valueLength = decodeU2(message, offset); + offset += 2; + String value = new String(message, offset, valueLength); + metadata.put(key, value); + offset += valueLength; + } + return metadata; + } + + private static int decodeU2(byte[] message, int offset) { + return ((0xFF & message[offset]) << 8) | (0xFF & message[offset + 1]); + } + + private static int decodeS4(byte[] message, int offset) { + return ((0xFF & message[offset]) << 24) | ((0xFF & message[offset + 1]) << 16) | ((0xFF & message[offset + 2]) << 8) + | (0xFF & message[offset + 3]); + } + + private static int getSupportedVersion(byte[] message) { + return message[2]; + } + + private static void encodeMetadata(int offset, byte[] message, Map metadata) { + for (Entry entry : metadata.entrySet()) { + String key = crop(MAX_U1, entry.getKey(), "metadata key"); + String value = crop(MAX_U2, entry.getValue(), "metadata value"); + message[offset++] = (byte) key.getBytes().length; + System.arraycopy(key.getBytes(), 0, message, offset, key.getBytes().length); + offset += key.getBytes().length; + encodeU2(message, offset, value.getBytes().length); + offset += 2; + System.arraycopy(value.getBytes(), 0, message, offset, value.getBytes().length); + offset += value.getBytes().length; + } + } + + private static String crop(int maxLength, String string, String entityName) { + if (string.getBytes().length > maxLength) { + LOGGER.error("The string for {} was too long for the protocol (max = {}) and has been cropped. The value started with {}", entityName, maxLength, string.substring(0, 20)); + return string.substring(0, maxLength); + } + return string; + } + + private static void encodeU2(byte[] buffer, int offset, int value) { + buffer[offset++] = (byte) ((value & 0x0000FF00) >> 8); + buffer[offset] = (byte) (value & 0x000000FF); + } + + private static void encodeU4(byte[] buffer, int offset, int value) { + buffer[offset++] = (byte) ((value & 0xFF000000) >> 24); + buffer[offset++] = (byte) ((value & 0x00FF0000) >> 16); + buffer[offset++] = (byte) ((value & 0x0000FF00) >> 8); + buffer[offset] = (byte) (value & 0x000000FF); + } + + private static int calculateEncodedLength(RoboContextDescriptor entry) { + return MAGIC.length + PROTOCOL_VERSION_BYTE_LENGTH + PACKAGE_LENGTH_BYTE_LENGTH + HEART_BEAT_PERIOD_BYTE_LENGTH + + ID_LENGTH_BYTE_LENGTH + entry.getId().getBytes().length + calculateMetadataByteLength(entry.getMetadata()); + } + + private static int calculateMetadataByteLength(Map metadata) { + return metadata.entrySet().stream().map(entry -> 1 + Math.min(entry.getKey().getBytes().length, MAX_U1) + 2 + + Math.min(entry.getValue().getBytes().length, MAX_U2)).reduce(0, Integer::sum); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/LookupServiceImpl.java b/robo4j-core/src/main/java/com/robo4j/net/LookupServiceImpl.java index b9600c87..570491f6 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/LookupServiceImpl.java +++ b/robo4j-core/src/main/java/com/robo4j/net/LookupServiceImpl.java @@ -17,17 +17,12 @@ package com.robo4j.net; import com.robo4j.RoboContext; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.net.LocalLookupServiceImpl.LocalRoboContextDescriptor; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.net.DatagramPacket; -import java.net.InetAddress; -import java.net.InetSocketAddress; -import java.net.MulticastSocket; -import java.net.SocketAddress; -import java.net.SocketException; -import java.net.UnknownHostException; +import java.net.*; import java.util.Collections; import java.util.HashMap; import java.util.Map; @@ -40,144 +35,144 @@ * heartbeats. Will automatically remove entries for contexts that have missed * enough heartbeats. Note that the entries themselves can have individual * settings for the heartbeat. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class LookupServiceImpl implements LookupService { - // FIXME(marcus/6 Nov 2017): This should be calculated, and used when - // constructing the packet - private final static int MAX_PACKET_SIZE = 1500; - private MulticastSocket socket; - private String address; - private int port; - private Updater currentUpdater; - private Map entries = new ConcurrentHashMap<>(); - private final LocalLookupServiceImpl localContexts; - - private class Updater implements Runnable { - private byte[] buffer = new byte[MAX_PACKET_SIZE]; - private volatile boolean isRunning; - - @Override - public void run() { - while (!isRunning) { - try { - DatagramPacket packet = new DatagramPacket(buffer, MAX_PACKET_SIZE); - socket.receive(packet); - process(packet); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), - "Failed to look for lookupservice packets. Lookup service will no longer discover new remote contexts.", e); - isRunning = false; - } - } - } - - private void process(DatagramPacket packet) { - // First a few quick checks. We want to reject updating anything as - // early as possible - if (!HearbeatMessageCodec.isHeartBeatMessage(packet.getData())) { - SimpleLoggingUtil.debug(getClass(), "Non-heartbeat packet sent to LookupService! Ignoring."); - return; - } - if (!HearbeatMessageCodec.isSupportedVersion(packet.getData())) { - SimpleLoggingUtil.debug(getClass(), "Wrong protocol heartbeat packet sent to LookupService! Ignoring."); - return; - } - String id = parseId(packet.getData()); - if (entries.containsKey(id)) { - updateEntry(entries.get(id)); - } else { - addNewEntry(packet); - } - } - - private void addNewEntry(DatagramPacket packet) { - RoboContextDescriptorEntry entry = parse(packet); - synchronized (LookupServiceImpl.this) { - entries.put(entry.descriptor.getId(), entry); - } - } - - private RoboContextDescriptorEntry parse(DatagramPacket packet) { - RoboContextDescriptorEntry entry = new RoboContextDescriptorEntry(); - SocketAddress address = packet.getSocketAddress(); - if (address instanceof InetSocketAddress) { - entry.address = ((InetSocketAddress) address).getAddress(); - } - entry.descriptor = HearbeatMessageCodec.decode(packet.getData()); - return entry; - } - - private void updateEntry(RoboContextDescriptorEntry roboContextDescriptorEntry) { - roboContextDescriptorEntry.lastAccess = System.currentTimeMillis(); - } - - private String parseId(byte[] data) { - return new String(data, 9, readShort(7, data)); - } - - private int readShort(int i, byte[] data) { - return (data[i] << 8) + (data[i + 1] & 0xFF); - } - - public void stop() { - isRunning = false; - } - } - - public LookupServiceImpl(String address, int port, float missedHeartbeatsBeforeRemoval, LocalLookupServiceImpl localContexts) - throws SocketException, UnknownHostException { - this.address = address; - this.port = port; - this.localContexts = localContexts; - - } - - @Override - public synchronized Map getDiscoveredContexts() { - Map map = new HashMap<>(entries.size() + localContexts.getDiscoveredContexts().size()); - map.putAll(localContexts.getDiscoveredContexts()); - for (Entry entry : entries.entrySet()) { - map.put(entry.getKey(), entry.getValue().descriptor); - } - return Collections.unmodifiableMap(map); - } - - @Override - public RoboContext getContext(String id) { - RoboContextDescriptorEntry entry = entries.get(id); - if (entry != null) { - return new ClientRemoteRoboContext(entry); - } else { - LocalRoboContextDescriptor localEntry = localContexts.getLocalDescriptor(id); - return localEntry != null ? localEntry.getContext() : null; - } - } - - @Override - public synchronized void start() throws IOException { - stop(); - socket = new MulticastSocket(port); - socket.joinGroup(InetAddress.getByName(address)); - currentUpdater = new Updater(); - Thread t = new Thread(currentUpdater, "LookupService listener"); - t.setDaemon(true); - t.start(); - } - - @Override - public synchronized void stop() { - if (currentUpdater != null) { - currentUpdater.stop(); - currentUpdater = null; - } - } - - @Override - public RoboContextDescriptor getDescriptor(String id) { - RoboContextDescriptorEntry entry = entries.get(id); - return entry != null ? entry.descriptor : localContexts.getDescriptor(id); - } + private static final Logger LOGGER = LoggerFactory.getLogger(LookupServiceImpl.class); + // FIXME(marcus/6 Nov 2017): This should be calculated, and used when + // constructing the packet + private final static int MAX_PACKET_SIZE = 1500; + private final String address; + private final int port; + private final Map entries = new ConcurrentHashMap<>(); + private MulticastSocket socket; + private Updater currentUpdater; + private final LocalLookupServiceImpl localContexts; + + private class Updater implements Runnable { + private final byte[] buffer = new byte[MAX_PACKET_SIZE]; + private volatile boolean isRunning; + + @Override + public void run() { + while (!isRunning) { + try { + DatagramPacket packet = new DatagramPacket(buffer, MAX_PACKET_SIZE); + socket.receive(packet); + process(packet); + } catch (IOException e) { + LOGGER.error("Failed to look for lookupservice packets. Lookup service will no longer discover new remote contexts.", e); + isRunning = false; + } + } + } + + private void process(DatagramPacket packet) { + // First a few quick checks. We want to reject updating anything as + // early as possible + if (!HearbeatMessageCodec.isHeartBeatMessage(packet.getData())) { + LOGGER.debug("Non-heartbeat packet sent to LookupService! Ignoring."); + return; + } + if (!HearbeatMessageCodec.isSupportedVersion(packet.getData())) { + LOGGER.debug("Wrong protocol heartbeat packet sent to LookupService! Ignoring."); + return; + } + String id = parseId(packet.getData()); + if (entries.containsKey(id)) { + updateEntry(entries.get(id)); + } else { + addNewEntry(packet); + } + } + + private void addNewEntry(DatagramPacket packet) { + RoboContextDescriptorEntry entry = parse(packet); + synchronized (LookupServiceImpl.this) { + entries.put(entry.descriptor.getId(), entry); + } + } + + private RoboContextDescriptorEntry parse(DatagramPacket packet) { + RoboContextDescriptorEntry entry = new RoboContextDescriptorEntry(); + SocketAddress address = packet.getSocketAddress(); + if (address instanceof InetSocketAddress) { + entry.address = ((InetSocketAddress) address).getAddress(); + } + entry.descriptor = HearbeatMessageCodec.decode(packet.getData()); + return entry; + } + + private void updateEntry(RoboContextDescriptorEntry roboContextDescriptorEntry) { + roboContextDescriptorEntry.lastAccess = System.currentTimeMillis(); + } + + private String parseId(byte[] data) { + return new String(data, 9, readShort(7, data)); + } + + private int readShort(int i, byte[] data) { + return (data[i] << 8) + (data[i + 1] & 0xFF); + } + + public void stop() { + isRunning = false; + } + } + + public LookupServiceImpl(String address, int port, float missedHeartbeatsBeforeRemoval, LocalLookupServiceImpl localContexts) + throws SocketException, UnknownHostException { + this.address = address; + this.port = port; + this.localContexts = localContexts; + + } + + @Override + public synchronized Map getDiscoveredContexts() { + Map map = new HashMap<>(entries.size() + localContexts.getDiscoveredContexts().size()); + map.putAll(localContexts.getDiscoveredContexts()); + for (Entry entry : entries.entrySet()) { + map.put(entry.getKey(), entry.getValue().descriptor); + } + return Collections.unmodifiableMap(map); + } + + @Override + public RoboContext getContext(String id) { + RoboContextDescriptorEntry entry = entries.get(id); + if (entry != null) { + return new ClientRemoteRoboContext(entry); + } else { + LocalRoboContextDescriptor localEntry = localContexts.getLocalDescriptor(id); + return localEntry != null ? localEntry.getContext() : null; + } + } + + @Override + public synchronized void start() throws IOException { + stop(); + socket = new MulticastSocket(port); + socket.joinGroup(InetAddress.getByName(address)); + currentUpdater = new Updater(); + Thread t = new Thread(currentUpdater, "LookupService listener"); + t.setDaemon(true); + t.start(); + } + + @Override + public synchronized void stop() { + if (currentUpdater != null) { + currentUpdater.stop(); + currentUpdater = null; + } + } + + @Override + public RoboContextDescriptor getDescriptor(String id) { + RoboContextDescriptorEntry entry = entries.get(id); + return entry != null ? entry.descriptor : localContexts.getDescriptor(id); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/MessageClient.java b/robo4j-core/src/main/java/com/robo4j/net/MessageClient.java index 96bcf047..6e657ef1 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/MessageClient.java +++ b/robo4j-core/src/main/java/com/robo4j/net/MessageClient.java @@ -18,13 +18,10 @@ import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.io.BufferedInputStream; -import java.io.BufferedOutputStream; -import java.io.IOException; -import java.io.ObjectInputStream; -import java.io.ObjectOutputStream; +import java.io.*; import java.net.Socket; import java.net.SocketTimeoutException; import java.net.URI; @@ -36,173 +33,173 @@ /** * Message client. Normally used by RemoteRoboContext to communicate with a * discovered MessageServer. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MessageClient { - public final static String KEY_SO_TIMEOUT = "timeout"; - public final static String KEY_KEEP_ALIVE = "keepAlive"; - public final static String KEY_RETRIES = "retries"; - public final static int DEFAULT_SO_TIMEOUT = 2000000; - public final static boolean DEFAULT_KEEP_ALIVE = true; - - /* - * Executor for incoming messages from the server - */ - private final ExecutorService remoteReferenceCallExecutor = Executors.newSingleThreadExecutor(new ThreadFactory() { - @Override - public Thread newThread(Runnable r) { - Thread t = new Thread(r, "RemoteReferenceCallExecutor for " + messageServerURI); - t.setDaemon(true); - return t; - } - }); - - /* - * Listening to incoming messages from the server, initiated by serialized - * robo references. - */ - private class RemoteReferenceListener implements Runnable { - private Socket socket; - private volatile boolean quit; - - public RemoteReferenceListener(Socket socket) { - this.socket = socket; - } - - @Override - public void run() { - ObjectInputStream ois = getStream(); - while (!quit) { - try { - String uuid = ois.readUTF(); - String id = ois.readUTF(); - Object message = ois.readObject(); - RoboContext context = LookupServiceProvider.getDefaultLookupService().getContext(uuid); - if (context == null) { - SimpleLoggingUtil.debug(MessageClient.class, - "Failed to find recipient context " + uuid + " for message " + message); - } else { - context.getReference(id).sendMessage(message); - } - } catch (SocketTimeoutException e) { - // This will likely happen. - SimpleLoggingUtil.error(getClass(), e.getMessage()); - } catch (Exception e) { - SimpleLoggingUtil.debug(MessageClient.class, "Message delivery failed for recipient", e); - } - } - } - - private ObjectInputStream getStream() { - try { - return new ObjectInputStream(new BufferedInputStream(socket.getInputStream())); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to get input stream for remote reference listener!"); - e.printStackTrace(); - } - return null; - } - - public void shutdown() { - quit = true; - } - - } - - private final URI messageServerURI; - private final String sourceUUID; - private Socket socket; - private ObjectOutputStream objectOutputStream; - private Configuration configuration; - private int failCount; - private final int maxFailCount; - private RemoteReferenceListener remoteReferenceListener; - - public MessageClient(URI messageServerURI, String sourceUUID, Configuration configuration) { - this.messageServerURI = messageServerURI; - this.sourceUUID = sourceUUID; - this.configuration = configuration; - this.maxFailCount = configuration.getInteger(KEY_RETRIES, 3); - } - - public void connect() throws UnknownHostException, IOException { - if (socket == null || socket.isClosed() || !socket.isConnected()) { + private static final Logger LOGGER = LoggerFactory.getLogger(MessageClient.class); + public final static String KEY_SO_TIMEOUT = "timeout"; + public final static String KEY_KEEP_ALIVE = "keepAlive"; + public final static String KEY_RETRIES = "retries"; + public final static int DEFAULT_SO_TIMEOUT = 2000000; + public final static boolean DEFAULT_KEEP_ALIVE = true; + + /* + * Executor for incoming messages from the server + */ + private final ExecutorService remoteReferenceCallExecutor = Executors.newSingleThreadExecutor(new ThreadFactory() { + @Override + public Thread newThread(Runnable r) { + Thread t = new Thread(r, "RemoteReferenceCallExecutor for " + messageServerURI); + t.setDaemon(true); + return t; + } + }); + + /* + * Listening to incoming messages from the server, initiated by serialized + * robo references. + */ + private static class RemoteReferenceListener implements Runnable { + private final Socket socket; + private volatile boolean quit; + + public RemoteReferenceListener(Socket socket) { + this.socket = socket; + } + + @Override + public void run() { + ObjectInputStream ois = getStream(); + while (!quit) { + try { + String uuid = ois.readUTF(); + String id = ois.readUTF(); + Object message = ois.readObject(); + RoboContext context = LookupServiceProvider.getDefaultLookupService().getContext(uuid); + if (context == null) { + LOGGER.debug("Failed to find recipient context {} for message {}", uuid, message); + } else { + context.getReference(id).sendMessage(message); + } + } catch (SocketTimeoutException e) { + // This will likely happen. + LOGGER.error(e.getMessage()); + } catch (Exception e) { + LOGGER.debug("Message delivery failed for recipient", e); + } + } + } + + private ObjectInputStream getStream() { + try { + return new ObjectInputStream(new BufferedInputStream(socket.getInputStream())); + } catch (IOException e) { + LOGGER.error("Failed to get input stream for remote reference listener!", e); + } + return null; + } + + public void shutdown() { + quit = true; + } + + } + + private final URI messageServerURI; + private final String sourceUUID; + private final Configuration configuration; + private Socket socket; + private ObjectOutputStream objectOutputStream; + private int failCount; + private final int maxFailCount; + private RemoteReferenceListener remoteReferenceListener; + + public MessageClient(URI messageServerURI, String sourceUUID, Configuration configuration) { + this.messageServerURI = messageServerURI; + this.sourceUUID = sourceUUID; + this.configuration = configuration; + this.maxFailCount = configuration.getInteger(KEY_RETRIES, 3); + } + + public void connect() throws UnknownHostException, IOException { + if (socket == null || socket.isClosed() || !socket.isConnected()) { // socket = new Socket(messageServerURI.getHost(), messageServerURI.getPort()); - socket = new Socket(messageServerURI.getHost(), messageServerURI.getPort()); - socket.setKeepAlive(configuration.getBoolean(KEY_KEEP_ALIVE, DEFAULT_KEEP_ALIVE)); - socket.setSoTimeout(configuration.getInteger(KEY_SO_TIMEOUT, DEFAULT_SO_TIMEOUT)); - } - objectOutputStream = new ObjectOutputStream(new BufferedOutputStream(socket.getOutputStream())); - objectOutputStream.writeShort(MessageProtocolConstants.MAGIC); - objectOutputStream.writeUTF(sourceUUID); - remoteReferenceListener = new RemoteReferenceListener(socket); - remoteReferenceCallExecutor.execute(remoteReferenceListener); - } - - public void sendMessage(String id, Object message) throws IOException { - try { - deliverMessage(id, message); - } catch (IOException e) { - if (failCount < maxFailCount) { - failCount++; - connect(); - sendMessage(id, message); - } else { - throw e; - } - } - } - - private void deliverMessage(String id, Object message) throws IOException { - objectOutputStream.writeUTF(id); - if (message instanceof String) { - objectOutputStream.writeByte(MessageProtocolConstants.MOD_UTF8); - objectOutputStream.writeUTF((String) message); - } else if (message instanceof Number) { - if (message instanceof Float) { - objectOutputStream.writeByte(MessageProtocolConstants.FLOAT); - objectOutputStream.writeFloat((Float) message); - } else if (message instanceof Integer) { - objectOutputStream.writeByte(MessageProtocolConstants.INT); - objectOutputStream.writeInt((Integer) message); - } else if (message instanceof Double) { - objectOutputStream.writeByte(MessageProtocolConstants.DOUBLE); - objectOutputStream.writeDouble((Double) message); - } else if (message instanceof Long) { - objectOutputStream.writeByte(MessageProtocolConstants.LONG); - objectOutputStream.writeLong((Long) message); - } else if (message instanceof Byte) { - objectOutputStream.writeByte(MessageProtocolConstants.BYTE); - objectOutputStream.writeByte((Byte) message); - } else if (message instanceof Short) { - objectOutputStream.writeByte(MessageProtocolConstants.SHORT); - objectOutputStream.writeShort((Short) message); - } - } else if (message instanceof Character) { - objectOutputStream.writeByte(MessageProtocolConstants.CHAR); - objectOutputStream.writeChar((Character) message); - } else { - objectOutputStream.writeByte(MessageProtocolConstants.OBJECT); - objectOutputStream.writeObject(message); - } - objectOutputStream.flush(); - } - - public boolean isConnected() { - return socket != null && socket.isConnected(); - } - - public void shutdown() { - try { - objectOutputStream.flush(); - objectOutputStream.close(); - remoteReferenceListener.shutdown(); - remoteReferenceCallExecutor.shutdown(); - socket.close(); - } catch (IOException e) { - // Do not care. - } - } + socket = new Socket(messageServerURI.getHost(), messageServerURI.getPort()); + socket.setKeepAlive(configuration.getBoolean(KEY_KEEP_ALIVE, DEFAULT_KEEP_ALIVE)); + socket.setSoTimeout(configuration.getInteger(KEY_SO_TIMEOUT, DEFAULT_SO_TIMEOUT)); + } + objectOutputStream = new ObjectOutputStream(new BufferedOutputStream(socket.getOutputStream())); + objectOutputStream.writeShort(MessageProtocolConstants.MAGIC); + objectOutputStream.writeUTF(sourceUUID); + remoteReferenceListener = new RemoteReferenceListener(socket); + remoteReferenceCallExecutor.execute(remoteReferenceListener); + } + + public void sendMessage(String id, Object message) throws IOException { + try { + deliverMessage(id, message); + } catch (IOException e) { + if (failCount < maxFailCount) { + failCount++; + connect(); + sendMessage(id, message); + } else { + throw e; + } + } + } + + private void deliverMessage(String id, Object message) throws IOException { + // TODO : replace by switch + objectOutputStream.writeUTF(id); + if (message instanceof String) { + objectOutputStream.writeByte(MessageProtocolConstants.MOD_UTF8); + objectOutputStream.writeUTF((String) message); + } else if (message instanceof Number) { + if (message instanceof Float) { + objectOutputStream.writeByte(MessageProtocolConstants.FLOAT); + objectOutputStream.writeFloat((Float) message); + } else if (message instanceof Integer) { + objectOutputStream.writeByte(MessageProtocolConstants.INT); + objectOutputStream.writeInt((Integer) message); + } else if (message instanceof Double) { + objectOutputStream.writeByte(MessageProtocolConstants.DOUBLE); + objectOutputStream.writeDouble((Double) message); + } else if (message instanceof Long) { + objectOutputStream.writeByte(MessageProtocolConstants.LONG); + objectOutputStream.writeLong((Long) message); + } else if (message instanceof Byte) { + objectOutputStream.writeByte(MessageProtocolConstants.BYTE); + objectOutputStream.writeByte((Byte) message); + } else if (message instanceof Short) { + objectOutputStream.writeByte(MessageProtocolConstants.SHORT); + objectOutputStream.writeShort((Short) message); + } + } else if (message instanceof Character) { + objectOutputStream.writeByte(MessageProtocolConstants.CHAR); + objectOutputStream.writeChar((Character) message); + } else { + objectOutputStream.writeByte(MessageProtocolConstants.OBJECT); + objectOutputStream.writeObject(message); + } + objectOutputStream.flush(); + } + + public boolean isConnected() { + return socket != null && socket.isConnected(); + } + + public void shutdown() { + try { + objectOutputStream.flush(); + objectOutputStream.close(); + remoteReferenceListener.shutdown(); + remoteReferenceCallExecutor.shutdown(); + socket.close(); + } catch (IOException e) { + // Do not care. + } + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/MessageServer.java b/robo4j-core/src/main/java/com/robo4j/net/MessageServer.java index fb395f45..586742af 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/MessageServer.java +++ b/robo4j-core/src/main/java/com/robo4j/net/MessageServer.java @@ -17,188 +17,177 @@ package com.robo4j.net; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.BufferedInputStream; import java.io.IOException; import java.io.ObjectInputStream; -import java.net.InetAddress; -import java.net.ServerSocket; -import java.net.Socket; -import java.net.URI; -import java.net.URISyntaxException; +import java.net.*; /** * This is a server that listens on messages, and sends them off to the * indicated local recipient. It is associated to RoboContext. - * + *

* TODO: Rewrite in NIO for better thread management. * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MessageServer { - public final static String KEY_HOST_NAME = "hostname"; - public static final String KEY_PORT = "port"; - - private volatile int listeningPort = 0; - private volatile String listeningHost; - private volatile boolean running = false; - private volatile Thread startingThread = null; - private MessageCallback callback; - private Configuration configuration; - - private class MessageHandler implements Runnable { - private Socket socket; - - public MessageHandler(Socket socket) { - this.socket = socket; - } - - @Override - public void run() { - try (ObjectInputStream objectInputStream = new ObjectInputStream( - new BufferedInputStream(socket.getInputStream()))) { - // Init protocol. First check magic... - if (checkMagic(objectInputStream.readShort())) { - final String uuid = objectInputStream.readUTF(); - final ServerRemoteRoboContext context = new ServerRemoteRoboContext(uuid, socket.getOutputStream()); - // Then keep reading string, byte, data triplets until dead - ReferenceDescriptor.setCurrentContext(context); - while (running) { - String id = (String) objectInputStream.readUTF(); - Object message = decodeMessage(objectInputStream); - callback.handleMessage(uuid, id, message); - } - } else { - SimpleLoggingUtil.error(getClass(), - "Got wrong communication magic - will shutdown communication with " - + socket.getRemoteSocketAddress()); - } - - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), - "IO Exception communicating with " + socket.getRemoteSocketAddress(), e); - } catch (ClassNotFoundException e) { - SimpleLoggingUtil.error(getClass(), - "Could not find class to deserialize message to - will stop receiving messages from " - + socket.getRemoteSocketAddress(), - e); - } - SimpleLoggingUtil.info(getClass(), "Shutting down socket " + socket.toString()); - - } - - private Object decodeMessage(ObjectInputStream objectInputStream) throws IOException, ClassNotFoundException { - byte dataType = objectInputStream.readByte(); - switch (dataType) { - case MessageProtocolConstants.OBJECT: - return objectInputStream.readObject(); - case MessageProtocolConstants.MOD_UTF8: - return objectInputStream.readUTF(); - case MessageProtocolConstants.BYTE: - return objectInputStream.readByte(); - case MessageProtocolConstants.SHORT: - return objectInputStream.readShort(); - case MessageProtocolConstants.FLOAT: - return objectInputStream.readFloat(); - case MessageProtocolConstants.INT: - return objectInputStream.readInt(); - case MessageProtocolConstants.DOUBLE: - return objectInputStream.readDouble(); - case MessageProtocolConstants.LONG: - return objectInputStream.readLong(); - case MessageProtocolConstants.CHAR: - return objectInputStream.readChar(); - default: - throw new IOException("The type with id " + dataType + " is not supported!"); - } - } - - private boolean checkMagic(short magic) { - return magic == MessageProtocolConstants.MAGIC; - } - } - - /** - * Constructor - * - * @param callback - * message callback - * - * @param configuration - * configuration - */ - public MessageServer(MessageCallback callback, Configuration configuration) { - this.callback = callback; - this.configuration = configuration; - } - - /** - * This will be blocking/running until stop is called (and perhaps for longer). - * Dispatch in whatever thread you feel appropriate. - * - * @throws IOException - * exception - */ - public void start() throws IOException { - startingThread = Thread.currentThread(); - String host = configuration.getString(KEY_HOST_NAME, null); - InetAddress bindAddress = null; - if (host != null) { - bindAddress = InetAddress.getByName(host); - } - - try (ServerSocket serverSocket = new ServerSocket(configuration.getInteger(KEY_PORT, 0), - configuration.getInteger("backlog", 20), bindAddress)) { - listeningHost = serverSocket.getInetAddress().getHostAddress(); - listeningPort = serverSocket.getLocalPort(); - ThreadGroup g = new ThreadGroup("Robo4J communication threads"); - running = true; - while (running) { - MessageHandler handler = new MessageHandler(serverSocket.accept()); - Thread t = new Thread(g, handler, "Communication [" + handler.socket.getRemoteSocketAddress() + "]"); - t.setDaemon(true); - t.start(); - } - } finally { - running = false; - startingThread = null; - } - } - - public void stop() { - running = false; - Thread startingThread = this.startingThread; - if (startingThread != null) { - startingThread.interrupt(); - } - } - - public int getListeningPort() { - return listeningPort; - } - - /** - * @return the URI for the listening socket. This is the address to connect to. - * Will return null if the server isn't up and running yet, or if badly - * configured. - */ - public URI getListeningURI() { - if (!running) { - return null; - } - - try { - String host = configuration.getString(KEY_HOST_NAME, null); - if (host != null) { - return new URI("robo4j", "", host, listeningPort, "", "", ""); - } else { - return new URI("robo4j", "", listeningHost, listeningPort, "", "", ""); - } - } catch (URISyntaxException e) { - SimpleLoggingUtil.error(getClass(), "Could not create URI for listening URI"); - return null; - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(MessageServer.class); + public final static String KEY_HOST_NAME = "hostname"; + public static final String KEY_PORT = "port"; + + private volatile int listeningPort = 0; + private volatile String listeningHost; + private volatile boolean running = false; + private volatile Thread startingThread = null; + private final MessageCallback callback; + private final Configuration configuration; + + private class MessageHandler implements Runnable { + private final Socket socket; + + public MessageHandler(Socket socket) { + this.socket = socket; + } + + @Override + public void run() { + try (ObjectInputStream objectInputStream = new ObjectInputStream( + new BufferedInputStream(socket.getInputStream()))) { + // Init protocol. First check magic... + if (checkMagic(objectInputStream.readShort())) { + final String uuid = objectInputStream.readUTF(); + final ServerRemoteRoboContext context = new ServerRemoteRoboContext(uuid, socket.getOutputStream()); + // Then keep reading string, byte, data triplets until dead + ReferenceDescriptor.setCurrentContext(context); + while (running) { + String id = objectInputStream.readUTF(); + Object message = decodeMessage(objectInputStream); + callback.handleMessage(uuid, id, message); + } + } else { + LOGGER.error("Got wrong communication magic - will shutdown communication with {}", socket.getRemoteSocketAddress()); + } + + } catch (IOException e) { + LOGGER.error("IO Exception communicating with {}", socket.getRemoteSocketAddress(), e); + } catch (ClassNotFoundException e) { + LOGGER.error("Could not find class to deserialize message to - will stop receiving messages from {}", socket.getRemoteSocketAddress(), e); + } + LOGGER.info("Shutting down socket {}", socket.toString()); + + } + + private Object decodeMessage(ObjectInputStream objectInputStream) throws IOException, ClassNotFoundException { + byte dataType = objectInputStream.readByte(); + // TODO: replace by better switch + switch (dataType) { + case MessageProtocolConstants.OBJECT: + return objectInputStream.readObject(); + case MessageProtocolConstants.MOD_UTF8: + return objectInputStream.readUTF(); + case MessageProtocolConstants.BYTE: + return objectInputStream.readByte(); + case MessageProtocolConstants.SHORT: + return objectInputStream.readShort(); + case MessageProtocolConstants.FLOAT: + return objectInputStream.readFloat(); + case MessageProtocolConstants.INT: + return objectInputStream.readInt(); + case MessageProtocolConstants.DOUBLE: + return objectInputStream.readDouble(); + case MessageProtocolConstants.LONG: + return objectInputStream.readLong(); + case MessageProtocolConstants.CHAR: + return objectInputStream.readChar(); + default: + throw new IOException("The type with id " + dataType + " is not supported!"); + } + } + + private boolean checkMagic(short magic) { + return magic == MessageProtocolConstants.MAGIC; + } + } + + /** + * Constructor + * + * @param callback message callback + * @param configuration configuration + */ + public MessageServer(MessageCallback callback, Configuration configuration) { + this.callback = callback; + this.configuration = configuration; + } + + /** + * This will be blocking/running until stop is called (and perhaps for longer). + * Dispatch in whatever thread you feel appropriate. + * + * @throws IOException exception + */ + public void start() throws IOException { + startingThread = Thread.currentThread(); + String host = configuration.getString(KEY_HOST_NAME, null); + InetAddress bindAddress = null; + if (host != null) { + bindAddress = InetAddress.getByName(host); + } + + try (ServerSocket serverSocket = new ServerSocket(configuration.getInteger(KEY_PORT, 0), + configuration.getInteger("backlog", 20), bindAddress)) { + listeningHost = serverSocket.getInetAddress().getHostAddress(); + listeningPort = serverSocket.getLocalPort(); + ThreadGroup g = new ThreadGroup("Robo4J communication threads"); + running = true; + while (running) { + MessageHandler handler = new MessageHandler(serverSocket.accept()); + Thread t = new Thread(g, handler, "Communication [" + handler.socket.getRemoteSocketAddress() + "]"); + t.setDaemon(true); + t.start(); + } + } finally { + running = false; + startingThread = null; + } + } + + public void stop() { + running = false; + Thread startingThread = this.startingThread; + if (startingThread != null) { + startingThread.interrupt(); + } + } + + public int getListeningPort() { + return listeningPort; + } + + /** + * @return the URI for the listening socket. This is the address to connect to. + * Will return null if the server isn't up and running yet, or if badly + * configured. + */ + public URI getListeningURI() { + if (!running) { + return null; + } + + try { + String host = configuration.getString(KEY_HOST_NAME, null); + if (host != null) { + return new URI("robo4j", "", host, listeningPort, "", "", ""); + } else { + return new URI("robo4j", "", listeningHost, listeningPort, "", "", ""); + } + } catch (URISyntaxException e) { + LOGGER.error("Could not create URI for listening URI"); + return null; + } + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/ReferenceDescriptor.java b/robo4j-core/src/main/java/com/robo4j/net/ReferenceDescriptor.java index ddf232b4..05735c1e 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/ReferenceDescriptor.java +++ b/robo4j-core/src/main/java/com/robo4j/net/ReferenceDescriptor.java @@ -16,9 +16,11 @@ */ package com.robo4j.net; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.ObjectStreamException; +import java.io.Serial; import java.io.Serializable; /** @@ -28,29 +30,32 @@ * @author Miroslav Wengner (@miragemiko) */ public class ReferenceDescriptor implements Serializable { - private static final long serialVersionUID = 1L; - private static final ThreadLocal ACTIVE_CONTEXT = new ThreadLocal<>(); + @Serial + private static final long serialVersionUID = 1L; + private static final Logger LOGGER = LoggerFactory.getLogger(ReferenceDescriptor.class); + private static final ThreadLocal ACTIVE_CONTEXT = new ThreadLocal<>(); - private final String ctxId; - private final String id; - private final String fqn; + private final String ctxId; + private final String id; + private final String fqn; - public ReferenceDescriptor(String ctxId, String id, String fqn) { - this.ctxId = ctxId; - this.id = id; - this.fqn = fqn; - } + public ReferenceDescriptor(String ctxId, String id, String fqn) { + this.ctxId = ctxId; + this.id = id; + this.fqn = fqn; + } - Object readResolve() throws ObjectStreamException { - ServerRemoteRoboContext remoteRoboContext = ACTIVE_CONTEXT.get(); - if (remoteRoboContext == null) { - SimpleLoggingUtil.error(getClass(), "No remote context set!"); - return null; - } - return remoteRoboContext.getRoboReference(ctxId, id, fqn); - } + @Serial + Object readResolve() throws ObjectStreamException { + ServerRemoteRoboContext remoteRoboContext = ACTIVE_CONTEXT.get(); + if (remoteRoboContext == null) { + LOGGER.error("No remote context set!"); + return null; + } + return remoteRoboContext.getRoboReference(ctxId, id, fqn); + } - public static void setCurrentContext(ServerRemoteRoboContext context) { - ACTIVE_CONTEXT.set(context); - } + public static void setCurrentContext(ServerRemoteRoboContext context) { + ACTIVE_CONTEXT.set(context); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/net/ServerRemoteRoboContext.java b/robo4j-core/src/main/java/com/robo4j/net/ServerRemoteRoboContext.java index 7275cfc4..cbb23c32 100644 --- a/robo4j-core/src/main/java/com/robo4j/net/ServerRemoteRoboContext.java +++ b/robo4j-core/src/main/java/com/robo4j/net/ServerRemoteRoboContext.java @@ -21,8 +21,9 @@ import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.scheduler.Scheduler; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.io.ObjectOutputStream; @@ -38,157 +39,157 @@ * @author Miroslav Wengner (@miragemiko) */ public class ServerRemoteRoboContext implements RoboContext { - private final String uuid; - private final ObjectOutputStream outputStream; - - @SuppressWarnings("rawtypes") - private class ServerRemoteRoboReference implements RoboReference { - /** - * Since references can come from any context discovered in the JVM or - * in the lookup services, we need to always track the context id to - * send off the message to. - */ - private final String ctxId; - private final String id; - private final Class actualMessageClass; - - public ServerRemoteRoboReference(String ctxId, String id, String fqn) { - this.ctxId = ctxId; - this.id = id; - this.actualMessageClass = resolve(fqn); - } - - public String getTargetContextId() { - return ctxId; - } - - @Override - public String getId() { - return id; - } - - @Override - public LifecycleState getState() { - // TODO Auto-generated method stub - return null; - } - - @Override - public Class getMessageType() { - return actualMessageClass; - } - - @Override - public Configuration getConfiguration() { - return null; - } - - @Override - public Collection> getKnownAttributes() { - // TODO Auto-generated method stub - return null; - } - - @Override - public Future, Object>> getAttributes() { - // TODO Auto-generated method stub - return null; - } - - @Override - public void sendMessage(Object message) { - try { - // FIXME: Change the serialization to be the same as for the - // client to server - outputStream.writeUTF(getTargetContextId()); - outputStream.writeUTF(getId()); - outputStream.writeObject(message); - } catch (IOException e) { - e.printStackTrace(); - } - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - // TODO Auto-generated method stub - return null; - } - - private Class resolve(String fqn) { - try { - return Class.forName(fqn); - } catch (ClassNotFoundException e) { - SimpleLoggingUtil.error(getClass(), "Could not use class for remote communication", e); - e.printStackTrace(); - } - return null; - } - - public String toString() { - return "[RemoteRef id: " + id + " server: " + uuid + " messageType: " + actualMessageClass.getName() + "]"; - } - } - - public ServerRemoteRoboContext(String uuid, OutputStream out) throws IOException { - this.uuid = uuid; - this.outputStream = new ObjectOutputStream(out); - } - - @Override - public LifecycleState getState() { - // TODO Auto-generated method stub - return null; - } - - @Override - public void shutdown() { - // TODO Auto-generated method stub - - } - - @Override - public void stop() { - // TODO Auto-generated method stub - - } - - @Override - public void start() { - // TODO Auto-generated method stub - - } - - @Override - public RoboReference getReference(String id) { - // TODO Auto-generated method stub - return null; - } - - @Override - public Collection> getUnits() { - // TODO Auto-generated method stub - return null; - } - - @Override - public Scheduler getScheduler() { - // TODO Auto-generated method stub - return null; - } - - @Override - public String getId() { - return uuid; - } - - @Override - public Configuration getConfiguration() { - // TODO Auto-generated method stub - return null; - } - - public RoboReference getRoboReference(String ctxId, String id, String fqn) { - // FIXME: Cache these? - return new ServerRemoteRoboReference(ctxId, id, fqn); - } + private static final Logger LOGGER = LoggerFactory.getLogger(ServerRemoteRoboContext.class); + private final String uuid; + private final ObjectOutputStream outputStream; + + @SuppressWarnings("rawtypes") + private class ServerRemoteRoboReference implements RoboReference { + /** + * Since references can come from any context discovered in the JVM or + * in the lookup services, we need to always track the context id to + * send off the message to. + */ + private final String ctxId; + private final String id; + private final Class actualMessageClass; + + public ServerRemoteRoboReference(String ctxId, String id, String fqn) { + this.ctxId = ctxId; + this.id = id; + this.actualMessageClass = resolve(fqn); + } + + public String getTargetContextId() { + return ctxId; + } + + @Override + public String getId() { + return id; + } + + @Override + public LifecycleState getState() { + // TODO Auto-generated method stub + return null; + } + + @Override + public Class getMessageType() { + return actualMessageClass; + } + + @Override + public Configuration getConfiguration() { + return null; + } + + @Override + public Collection> getKnownAttributes() { + // TODO Auto-generated method stub + return null; + } + + @Override + public Future, Object>> getAttributes() { + // TODO Auto-generated method stub + return null; + } + + @Override + public void sendMessage(Object message) { + try { + // FIXME: Change the serialization to be the same as for the + // client to server + outputStream.writeUTF(getTargetContextId()); + outputStream.writeUTF(getId()); + outputStream.writeObject(message); + } catch (IOException e) { + LOGGER.error("send message:{}", message, e); + } + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + // TODO Auto-generated method stub + return null; + } + + private Class resolve(String fqn) { + try { + return Class.forName(fqn); + } catch (ClassNotFoundException e) { + LOGGER.error("Could not use class for remote communication", e); + } + return null; + } + + public String toString() { + return "[RemoteRef id: " + id + " server: " + uuid + " messageType: " + actualMessageClass.getName() + "]"; + } + } + + public ServerRemoteRoboContext(String uuid, OutputStream out) throws IOException { + this.uuid = uuid; + this.outputStream = new ObjectOutputStream(out); + } + + @Override + public LifecycleState getState() { + // TODO Auto-generated method stub + return null; + } + + @Override + public void shutdown() { + // TODO Auto-generated method stub + + } + + @Override + public void stop() { + // TODO Auto-generated method stub + + } + + @Override + public void start() { + // TODO Auto-generated method stub + + } + + @Override + public RoboReference getReference(String id) { + // TODO Auto-generated method stub + return null; + } + + @Override + public Collection> getUnits() { + // TODO Auto-generated method stub + return null; + } + + @Override + public Scheduler getScheduler() { + // TODO Auto-generated method stub + return null; + } + + @Override + public String getId() { + return uuid; + } + + @Override + public Configuration getConfiguration() { + // TODO Auto-generated method stub + return null; + } + + public RoboReference getRoboReference(String ctxId, String id, String fqn) { + // FIXME: Cache these? + return new ServerRemoteRoboReference(ctxId, id, fqn); + } } diff --git a/robo4j-core/src/main/java/module-info.java b/robo4j-core/src/main/java/module-info.java index 174237f7..58faca17 100644 --- a/robo4j-core/src/main/java/module-info.java +++ b/robo4j-core/src/main/java/module-info.java @@ -21,6 +21,7 @@ module robo4j.core { requires java.logging; requires java.xml; + requires org.slf4j; exports com.robo4j; exports com.robo4j.util; diff --git a/robo4j-core/src/test/java/com/robo4j/CounterUnitTests.java b/robo4j-core/src/test/java/com/robo4j/CounterUnitTests.java index 90717754..1a86041f 100644 --- a/robo4j-core/src/test/java/com/robo4j/CounterUnitTests.java +++ b/robo4j-core/src/test/java/com/robo4j/CounterUnitTests.java @@ -26,9 +26,7 @@ import java.util.ArrayList; import java.util.concurrent.ExecutionException; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * Test for the CounterUnit. @@ -37,48 +35,50 @@ * @author Miroslav Wengner (@miragemiko) */ class CounterUnitTests { - private static final String ID_COUNTER = "counter"; - private static final String ID_CONSUMER = "consumer"; - private static final AttributeDescriptor NUMBER_OF_MESSAGES = new DefaultAttributeDescriptor<>(Integer.class, - "NumberOfReceivedMessages"); - private static final AttributeDescriptor COUNTER = new DefaultAttributeDescriptor<>(Integer.class, "Counter"); + private static final String ID_COUNTER = "counter"; + private static final String ID_CONSUMER = "consumer"; + private static final AttributeDescriptor NUMBER_OF_MESSAGES = new DefaultAttributeDescriptor<>(Integer.class, + "NumberOfReceivedMessages"); + private static final AttributeDescriptor COUNTER = new DefaultAttributeDescriptor<>(Integer.class, "Counter"); - @SuppressWarnings({ "unchecked", "rawtypes" }) - private static final AttributeDescriptor> MESSAGES = new DefaultAttributeDescriptor>( - (Class>) new ArrayList().getClass(), "ReceivedMessages"); + @SuppressWarnings({"unchecked", "rawtypes"}) + private static final AttributeDescriptor> MESSAGES = new DefaultAttributeDescriptor>( + (Class>) new ArrayList().getClass(), "ReceivedMessages"); - @Test - void test() throws RoboBuilderException, InterruptedException, ExecutionException { - // FIXME(Marcus/Aug 20, 2017): We really should get rid of the sleeps - // here and use waits with timeouts... - RoboBuilder builder = new RoboBuilder(); - builder.add(IntegerConsumer.class, ID_CONSUMER); - builder.add(CounterUnit.class, getCounterConfiguration(ID_CONSUMER, 1000), ID_COUNTER); - RoboContext context = builder.build(); - context.start(); - assertEquals(LifecycleState.STARTED, context.getState()); - RoboReference counter = context.getReference(ID_COUNTER); - RoboReference consumer = context.getReference(ID_CONSUMER); - counter.sendMessage(CounterCommand.START); - Thread.sleep(2500); - assertTrue(consumer.getAttribute(NUMBER_OF_MESSAGES).get() > 2); - counter.sendMessage(CounterCommand.STOP); - Thread.sleep(200); - Integer count = consumer.getAttribute(NUMBER_OF_MESSAGES).get(); - Thread.sleep(2500); - assertEquals(count, consumer.getAttribute(NUMBER_OF_MESSAGES).get()); - ArrayList messages = consumer.getAttribute(MESSAGES).get(); - assertNotEquals(0, messages.size()); - assertNotEquals(0, (int) messages.get(messages.size() - 1)); - counter.sendMessage(CounterCommand.RESET); - Thread.sleep(1000); - assertEquals(0, (int) counter.getAttribute(COUNTER).get()); - } - private Configuration getCounterConfiguration(String target, int interval) { - Configuration configuration = new ConfigurationBuilder().addString(CounterUnit.KEY_TARGET, target) - .addInteger(CounterUnit.KEY_INTERVAL, interval).build(); - return configuration; - } + // TODO : review the test and comment, simplify it + @Test + void test() throws RoboBuilderException, InterruptedException, ExecutionException { + // FIXME(Marcus/Aug 20, 2017): We really should get rid of the sleeps + // here and use waits with timeouts... + RoboBuilder builder = new RoboBuilder(); + builder.add(IntegerConsumer.class, ID_CONSUMER); + builder.add(CounterUnit.class, getCounterConfiguration(ID_CONSUMER, 1000), ID_COUNTER); + RoboContext context = builder.build(); + context.start(); + assertEquals(LifecycleState.STARTED, context.getState()); + RoboReference counter = context.getReference(ID_COUNTER); + RoboReference consumer = context.getReference(ID_CONSUMER); + counter.sendMessage(CounterCommand.START); + Thread.sleep(2500); + assertTrue(consumer.getAttribute(NUMBER_OF_MESSAGES).get() > 2); + counter.sendMessage(CounterCommand.STOP); + Thread.sleep(200); + Integer count = consumer.getAttribute(NUMBER_OF_MESSAGES).get(); + Thread.sleep(2500); + assertEquals(count, consumer.getAttribute(NUMBER_OF_MESSAGES).get()); + ArrayList messages = consumer.getAttribute(MESSAGES).get(); + assertNotEquals(0, messages.size()); + assertNotEquals(0, (int) messages.get(messages.size() - 1)); + counter.sendMessage(CounterCommand.RESET); + Thread.sleep(1000); + assertEquals(0, (int) counter.getAttribute(COUNTER).get()); + } + + private Configuration getCounterConfiguration(String target, int interval) { + Configuration configuration = new ConfigurationBuilder().addString(CounterUnit.KEY_TARGET, target) + .addInteger(CounterUnit.KEY_INTERVAL, interval).build(); + return configuration; + } } From b450f38c4fc591c1b8ed775c67fe6cd6a9571c45 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Mon, 7 Oct 2024 09:06:18 +0200 Subject: [PATCH 02/13] [69] robo4j-core test logger --- .../com/robo4j/reflect/ReflectionScan.java | 28 +- .../robo4j/scheduler/DefaultScheduler.java | 196 +++--- .../java/com/robo4j/units/CounterUnit.java | 203 +++---- .../java/com/robo4j/util/StreamUtils.java | 135 ++--- .../main/java/com/robo4j/util/SystemUtil.java | 98 +-- .../ConfigurationBuilderTest.java | 50 +- .../configuration/ConfigurationTest.java | 49 +- .../RemoteSystemBuilderTest.java | 20 +- .../XmlConfigurationFactoryTest.java | 50 +- .../com/robo4j/net/LookupServiceTests.java | 108 ++-- .../com/robo4j/net/MessageServerTest.java | 328 +++++----- .../com/robo4j/net/RemoteContextTests.java | 568 +++++++++--------- .../com/robo4j/net/RemoteStringProducer.java | 25 +- .../robo4j/net/RemoteTestMessageProducer.java | 38 +- .../robo4j/units/ConfigurationConsumer.java | 84 +-- .../java/com/robo4j/units/StringProducer.java | 8 +- .../robo4j/units/StringProducerRemote.java | 6 +- .../robo4j/units/StringScheduledEmitter.java | 8 +- 18 files changed, 997 insertions(+), 1005 deletions(-) diff --git a/robo4j-core/src/main/java/com/robo4j/reflect/ReflectionScan.java b/robo4j-core/src/main/java/com/robo4j/reflect/ReflectionScan.java index 8ce153d2..64c25b6d 100644 --- a/robo4j-core/src/main/java/com/robo4j/reflect/ReflectionScan.java +++ b/robo4j-core/src/main/java/com/robo4j/reflect/ReflectionScan.java @@ -16,20 +16,17 @@ */ package com.robo4j.reflect; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.util.StreamUtils; import com.robo4j.util.StringConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.File; import java.io.FileInputStream; import java.io.FileNotFoundException; import java.io.IOException; import java.net.URL; -import java.util.ArrayList; -import java.util.Collections; -import java.util.Enumeration; -import java.util.List; -import java.util.Objects; +import java.util.*; import java.util.stream.Collectors; import java.util.stream.Stream; import java.util.zip.ZipEntry; @@ -42,19 +39,20 @@ * @author Miro Wengner (@miragemiko) */ public final class ReflectionScan { + private static final Logger LOGGER = LoggerFactory.getLogger(ReflectionScan.class); private static final String FILE = "file:"; private static final String SUFFIX = ".class"; - private static final String EXCLAMATION = "\u0021"; //Exclamation mark ! + private static final String EXCLAMATION = "\u0021"; //Exclamation mark ! private static final char SLASH = '/'; private static final char DOT = '.'; private final ClassLoader loader; - public ReflectionScan(ClassLoader loader){ + public ReflectionScan(ClassLoader loader) { this.loader = loader; } - public List scanForEntities(String... entityPackages){ + public List scanForEntities(String... entityPackages) { final List result = new ArrayList<>(); for (String packageName : entityPackages) { packageName = packageName.trim(); @@ -63,8 +61,7 @@ public List scanForEntities(String... entityPackages){ if (classesInPackage.isEmpty()) { classesInPackage.addAll(scanPackageOnDisk(loader, packageName)); if (classesInPackage.isEmpty()) { - SimpleLoggingUtil.debug(getClass(), - "We did not find any annotated classes in package " + packageName); + LOGGER.debug("We did not find any annotated classes in package {}", packageName); } else { result.addAll(classesInPackage); } @@ -72,8 +69,7 @@ public List scanForEntities(String... entityPackages){ result.addAll(classesInPackage); } } catch (IOException e) { - // TODO: provide robust logging - e.printStackTrace(); + LOGGER.error(e.getMessage(), e); } } return result; @@ -114,7 +110,7 @@ private List scanJarPackage(ClassLoader loader, String packageName) thro } private List scanPackageOnDisk(ClassLoader loader, String packageName) throws IOException { - final var packagePathStr = slashify(packageName); + final var packagePathStr = slashify(packageName); // TODO: improve reflection, review usage return loader.resources(packagePathStr).map(URL::getFile).map(File::new) .map(f -> findClasses(f, packageName)).flatMap(List::stream).collect(Collectors.toList()); @@ -135,8 +131,8 @@ private List findClassesIntern(File dir, String path) { } else if (file.getName().endsWith(SUFFIX)) { final StringBuilder sb = new StringBuilder(); sb.append(path.replace(File.separatorChar, DOT)) - .append(DOT) - .append(file.getName(), 0, file.getName().length() - SUFFIX.length()); + .append(DOT) + .append(file.getName(), 0, file.getName().length() - SUFFIX.length()); result.add(sb.toString()); } diff --git a/robo4j-core/src/main/java/com/robo4j/scheduler/DefaultScheduler.java b/robo4j-core/src/main/java/com/robo4j/scheduler/DefaultScheduler.java index 060249ba..c0e644b9 100644 --- a/robo4j-core/src/main/java/com/robo4j/scheduler/DefaultScheduler.java +++ b/robo4j-core/src/main/java/com/robo4j/scheduler/DefaultScheduler.java @@ -16,115 +16,109 @@ */ package com.robo4j.scheduler; -import java.util.concurrent.Callable; -import java.util.concurrent.Future; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.ScheduledThreadPoolExecutor; -import java.util.concurrent.TimeUnit; - import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.RoboUnit; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.concurrent.*; /** * This is the default scheduler used in Robo4J. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class DefaultScheduler implements Scheduler { - private static final int DEFAULT_NUMBER_OF_THREADS = 2; - private static final int TERMINATION_TIMEOUT_SEC = 4; - - private final ScheduledExecutorService executor; - private final RoboContext context; - - /** - * Default constructor. - * - * @param context - * the context. - */ - public DefaultScheduler(RoboContext context) { - this(context, DEFAULT_NUMBER_OF_THREADS); - } - - /** - * Constructor. - * - * @param context - * the context. - * @param numberOfThreads - * the number of threads in the thread pool. - */ - public DefaultScheduler(RoboContext context, int numberOfThreads) { - this.context = context; - this.executor = new ScheduledThreadPoolExecutor(numberOfThreads, - new RoboThreadFactory(new ThreadGroup("Robo4J Scheduler"), "Robo4J Scheduler", true)); - } - - @Override - public ScheduledFuture schedule(RoboReference target, T message, long delay, long interval, TimeUnit unit, - int numberOfInvocations) { - return schedule(target, message, delay, interval, unit, numberOfInvocations, null); - } - - @Override - public ScheduledFuture schedule(RoboReference target, T message, long delay, long period, TimeUnit unit, - int numberOfInvocations, FinalInvocationListener listener) { - ScheduledMessageWrapper command = createCommand(target, numberOfInvocations, message, listener); - ScheduledFuture future = executor.scheduleAtFixedRate(command, delay, period, unit); - command.setFuture(future); - return future; - } - - private ScheduledMessageWrapper createCommand(RoboReference reference, int numberOfInvocations, T message, - FinalInvocationListener listener) { - return new ScheduledMessageWrapper<>(context, reference, numberOfInvocations, message, listener); - } - - @Override - public ScheduledFuture schedule(RoboReference target, T message, long delay, long interval, TimeUnit unit) { - return executor.scheduleAtFixedRate(() -> deliverMessage(target, message), delay, interval, unit); - } - - @Override - public void shutdown() throws InterruptedException { - executor.shutdown(); - var status = executor.awaitTermination(TERMINATION_TIMEOUT_SEC, TimeUnit.SECONDS); - SimpleLoggingUtil.debug(DefaultScheduler.class, "shutdown status:" + status); - } - - static void deliverMessage(final RoboReference reference, final T message) { - // Performance optimization - let the scheduling thread deliver the - // message directly if this is robo unit implementation, instead of - // enqueuing it with the message executor. - if (reference instanceof RoboUnit) { - ((RoboUnit) reference).onMessage(message); - } else { - reference.sendMessage(message); - } - } - - @Override - public ScheduledFuture scheduleAtFixedRate(Runnable runnable, long delay, long interval, TimeUnit unit) { - return executor.scheduleAtFixedRate(runnable, delay, interval, unit); - } - - @Override - public void schedule(Runnable runnable, long delay, TimeUnit unit) { - executor.schedule(runnable, delay, unit); - } - - @Override - public void execute(Runnable r) { - executor.execute(r); - } - - @Override - public Future submit(Callable r) { - return executor.submit(r); - } + private static final Logger LOGGER = LoggerFactory.getLogger(DefaultScheduler.class); + private static final int DEFAULT_NUMBER_OF_THREADS = 2; + private static final int TERMINATION_TIMEOUT_SEC = 4; + + private final ScheduledExecutorService executor; + private final RoboContext context; + + /** + * Default constructor. + * + * @param context the context. + */ + public DefaultScheduler(RoboContext context) { + this(context, DEFAULT_NUMBER_OF_THREADS); + } + + /** + * Constructor. + * + * @param context the context. + * @param numberOfThreads the number of threads in the thread pool. + */ + public DefaultScheduler(RoboContext context, int numberOfThreads) { + this.context = context; + this.executor = new ScheduledThreadPoolExecutor(numberOfThreads, + new RoboThreadFactory(new ThreadGroup("Robo4J Scheduler"), "Robo4J Scheduler", true)); + } + + @Override + public ScheduledFuture schedule(RoboReference target, T message, long delay, long interval, TimeUnit unit, + int numberOfInvocations) { + return schedule(target, message, delay, interval, unit, numberOfInvocations, null); + } + + @Override + public ScheduledFuture schedule(RoboReference target, T message, long delay, long period, TimeUnit unit, + int numberOfInvocations, FinalInvocationListener listener) { + ScheduledMessageWrapper command = createCommand(target, numberOfInvocations, message, listener); + ScheduledFuture future = executor.scheduleAtFixedRate(command, delay, period, unit); + command.setFuture(future); + return future; + } + + private ScheduledMessageWrapper createCommand(RoboReference reference, int numberOfInvocations, T message, + FinalInvocationListener listener) { + return new ScheduledMessageWrapper<>(context, reference, numberOfInvocations, message, listener); + } + + @Override + public ScheduledFuture schedule(RoboReference target, T message, long delay, long interval, TimeUnit unit) { + return executor.scheduleAtFixedRate(() -> deliverMessage(target, message), delay, interval, unit); + } + + @Override + public void shutdown() throws InterruptedException { + executor.shutdown(); + var status = executor.awaitTermination(TERMINATION_TIMEOUT_SEC, TimeUnit.SECONDS); + LOGGER.debug("shutdown status:{}", status); + } + + static void deliverMessage(final RoboReference reference, final T message) { + // Performance optimization - let the scheduling thread deliver the + // message directly if this is robo unit implementation, instead of + // enqueuing it with the message executor. + if (reference instanceof RoboUnit) { + ((RoboUnit) reference).onMessage(message); + } else { + reference.sendMessage(message); + } + } + + @Override + public ScheduledFuture scheduleAtFixedRate(Runnable runnable, long delay, long interval, TimeUnit unit) { + return executor.scheduleAtFixedRate(runnable, delay, interval, unit); + } + + @Override + public void schedule(Runnable runnable, long delay, TimeUnit unit) { + executor.schedule(runnable, delay, unit); + } + + @Override + public void execute(Runnable r) { + executor.execute(r); + } + + @Override + public Future submit(Callable r) { + return executor.submit(r); + } } diff --git a/robo4j-core/src/main/java/com/robo4j/units/CounterUnit.java b/robo4j-core/src/main/java/com/robo4j/units/CounterUnit.java index 76d2ff54..e536aa15 100644 --- a/robo4j-core/src/main/java/com/robo4j/units/CounterUnit.java +++ b/robo4j-core/src/main/java/com/robo4j/units/CounterUnit.java @@ -16,13 +16,10 @@ */ package com.robo4j.units; -import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; -import com.robo4j.RoboContext; -import com.robo4j.RoboReference; -import com.robo4j.RoboUnit; +import com.robo4j.*; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.ScheduledFuture; import java.util.concurrent.TimeUnit; @@ -31,107 +28,105 @@ /** * A simple unit which will count upwards from zero. Useful, for example, as a * heart beat generator. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class CounterUnit extends RoboUnit { - private final AtomicInteger counter = new AtomicInteger(0); - - private int interval; - - /** - * This configuration key controls the interval between the updates, in ms. - */ - public static final String KEY_INTERVAL = "interval"; - - /** - * The default period, if no period is configured. - */ - public static final int DEFAULT_INTERVAL = 1000; - - /** - * This configuration key controls the target of the counter updates. This - * configuration key is mandatory. Also, the target must exist when the - * counter unit is started, and any change whilst running will be ignored. - */ - public static final String KEY_TARGET = "target"; - - /* - * The currently running timer updater. - */ - private ScheduledFuture scheduledFuture; - - /* - * The id of the target. - */ - private String targetId; - - private final class CounterUnitAction implements Runnable { - private RoboReference target; - - public CounterUnitAction(RoboReference target) { - this.target = target; - } - - @Override - public void run() { - if (target != null) { - target.sendMessage(counter.getAndIncrement()); - } else { - SimpleLoggingUtil.error(CounterUnit.class, - "The target " + targetId + " for the CounterUnit does not exist! Could not send count!"); - } - } - } - - /** - * Constructor. - * - * @param context - * the RoboContext. - * @param id - * the id of the RoboUnit. - */ - public CounterUnit(RoboContext context, String id) { - super(CounterCommand.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - interval = configuration.getInteger(KEY_INTERVAL, DEFAULT_INTERVAL); - targetId = configuration.getString(KEY_TARGET, null); - if (targetId == null) { - throw ConfigurationException.createMissingConfigNameException(KEY_TARGET); - } - } - - @Override - public void onMessage(CounterCommand message) { - synchronized (this) { - super.onMessage(message); - switch (message) { - case START: - scheduledFuture = getContext().getScheduler().scheduleAtFixedRate( - new CounterUnitAction(getContext().getReference(targetId)), 0, interval, TimeUnit.MILLISECONDS); - break; - case STOP: - scheduledFuture.cancel(false); - break; - case RESET: - counter.set(0); - break; - } - } - } - - @SuppressWarnings("unchecked") - @Override - public synchronized R onGetAttribute(AttributeDescriptor attribute) { - if (attribute.getAttributeName().equals("Counter") && attribute.getAttributeType() == Integer.class) { - return (R) (Integer) counter.get(); - } - return null; - } + private static final Logger LOGGER = LoggerFactory.getLogger(CounterUnit.class); + private final AtomicInteger counter = new AtomicInteger(0); + + private int interval; + + /** + * This configuration key controls the interval between the updates, in ms. + */ + public static final String KEY_INTERVAL = "interval"; + + /** + * The default period, if no period is configured. + */ + public static final int DEFAULT_INTERVAL = 1000; + + /** + * This configuration key controls the target of the counter updates. This + * configuration key is mandatory. Also, the target must exist when the + * counter unit is started, and any change whilst running will be ignored. + */ + public static final String KEY_TARGET = "target"; + + /* + * The currently running timer updater. + */ + private ScheduledFuture scheduledFuture; + + /* + * The id of the target. + */ + private String targetId; + + private final class CounterUnitAction implements Runnable { + private final RoboReference target; + + public CounterUnitAction(RoboReference target) { + this.target = target; + } + + @Override + public void run() { + if (target != null) { + target.sendMessage(counter.getAndIncrement()); + } else { + LOGGER.error("The target {} for the CounterUnit does not exist! Could not send count!", targetId); + } + } + } + + /** + * Constructor. + * + * @param context the RoboContext. + * @param id the id of the RoboUnit. + */ + public CounterUnit(RoboContext context, String id) { + super(CounterCommand.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + interval = configuration.getInteger(KEY_INTERVAL, DEFAULT_INTERVAL); + targetId = configuration.getString(KEY_TARGET, null); + if (targetId == null) { + throw ConfigurationException.createMissingConfigNameException(KEY_TARGET); + } + } + + @Override + public void onMessage(CounterCommand message) { + synchronized (this) { + super.onMessage(message); + switch (message) { + case START: + scheduledFuture = getContext().getScheduler().scheduleAtFixedRate( + new CounterUnitAction(getContext().getReference(targetId)), 0, interval, TimeUnit.MILLISECONDS); + break; + case STOP: + scheduledFuture.cancel(false); + break; + case RESET: + counter.set(0); + break; + } + } + } + + @SuppressWarnings("unchecked") + @Override + public synchronized R onGetAttribute(AttributeDescriptor attribute) { + if (attribute.getAttributeName().equals("Counter") && attribute.getAttributeType() == Integer.class) { + return (R) (Integer) counter.get(); + } + return null; + } } diff --git a/robo4j-core/src/main/java/com/robo4j/util/StreamUtils.java b/robo4j-core/src/main/java/com/robo4j/util/StreamUtils.java index b792bf2d..b3c283b6 100644 --- a/robo4j-core/src/main/java/com/robo4j/util/StreamUtils.java +++ b/robo4j-core/src/main/java/com/robo4j/util/StreamUtils.java @@ -16,7 +16,8 @@ */ package com.robo4j.util; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.ByteArrayOutputStream; import java.io.InputStream; @@ -29,83 +30,75 @@ /** * Streams related utils - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class StreamUtils { - public static final int STREAM_END = -1; + private static final Logger LOGGER = LoggerFactory.getLogger(StreamUtils.class); + public static final int STREAM_END = -1; - /** - * convert Enumeration to the Stream - * - * @param e - * enumeration of element E - * @param parallel - * parallel - * @param - * element instance - * @return stream of elements E - */ - public static Stream streamOfEnumeration(Enumeration e, boolean parallel) { - return StreamSupport.stream(Spliterators.spliteratorUnknownSize(new Iterator() { - public E next() { - return e.nextElement(); - } + /** + * convert Enumeration to the Stream + * + * @param e enumeration of element E + * @param parallel parallel + * @param element instance + * @return stream of elements E + */ + public static Stream streamOfEnumeration(Enumeration e, boolean parallel) { + return StreamSupport.stream(Spliterators.spliteratorUnknownSize(new Iterator() { + public E next() { + return e.nextElement(); + } - public boolean hasNext() { - return e.hasMoreElements(); - } - }, Spliterator.ORDERED), parallel); - } + public boolean hasNext() { + return e.hasMoreElements(); + } + }, Spliterator.ORDERED), parallel); + } - /** - * convert iterable to the stream - * - * @param iterable - * iterable of element E - * @param parallel - * parallel - * @param - * element instance - * @return Stream of elements - */ - public static Stream streamOf(Iterable iterable, boolean parallel) { - return StreamSupport.stream(iterable.spliterator(), parallel); - } + /** + * convert iterable to the stream + * + * @param iterable iterable of element E + * @param parallel parallel + * @param element instance + * @return Stream of elements + */ + public static Stream streamOf(Iterable iterable, boolean parallel) { + return StreamSupport.stream(iterable.spliterator(), parallel); + } - /** - * convert stream to iterable - * - * @param stream - * stream of element E - * @param - * element instance - * @return iterable of elements E - */ - public static Iterable iterableOf(Stream stream) { - return stream::iterator; - } + /** + * convert stream to iterable + * + * @param stream stream of element E + * @param element instance + * @return iterable of elements E + */ + public static Iterable iterableOf(Stream stream) { + return stream::iterator; + } - /** - * converting input stream to byte array - * - * @param inputStream - * input stream - * @return byte array - */ - public static byte[] inputStreamToByteArray(InputStream inputStream) { - try (final ByteArrayOutputStream baos = new ByteArrayOutputStream()) { - int imageCh; - while ((imageCh = inputStream.read()) != STREAM_END) { - baos.write(imageCh); - } - inputStream.close(); - baos.flush(); - return baos.toByteArray(); - } catch (Exception e) { - SimpleLoggingUtil.error(StreamUtils.class, e.getMessage()); - return new byte[0]; - } - } + /** + * converting input stream to byte array + * + * @param inputStream input stream + * @return byte array + */ + public static byte[] inputStreamToByteArray(InputStream inputStream) { + try (final ByteArrayOutputStream baos = new ByteArrayOutputStream()) { + int imageCh; + while ((imageCh = inputStream.read()) != STREAM_END) { + baos.write(imageCh); + } + inputStream.close(); + baos.flush(); + return baos.toByteArray(); + } catch (Exception e) { + LOGGER.error(e.getMessage(), e); + return new byte[0]; + } + } } diff --git a/robo4j-core/src/main/java/com/robo4j/util/SystemUtil.java b/robo4j-core/src/main/java/com/robo4j/util/SystemUtil.java index 4918a00e..adb2acb0 100644 --- a/robo4j-core/src/main/java/com/robo4j/util/SystemUtil.java +++ b/robo4j-core/src/main/java/com/robo4j/util/SystemUtil.java @@ -18,7 +18,8 @@ import com.robo4j.RoboContext; import com.robo4j.RoboReference; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.InputStream; import java.util.ArrayList; @@ -27,47 +28,48 @@ /** * Some useful little utilities. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class SystemUtil { - public static final String BREAK = "\n"; - public static final String DELIMITER_HORIZONTAL = "================================================"; - private static final String SLASH = "/"; + private static final Logger LOGGER = LoggerFactory.getLogger(SystemUtil.class); + public static final String BREAK = "\n"; + public static final String DELIMITER_HORIZONTAL = "================================================"; + private static final String SLASH = "/"; - private SystemUtil() { - // no instances - } + private SystemUtil() { + // no instances + } - public static final Comparator> ID_COMPARATOR = Comparator.comparing(RoboReference::getId); + public static final Comparator> ID_COMPARATOR = Comparator.comparing(RoboReference::getId); - public static InputStream getInputStreamByResourceName(String resourceName){ - return Thread.currentThread().getContextClassLoader().getResourceAsStream(resourceName); - } + public static InputStream getInputStreamByResourceName(String resourceName) { + return Thread.currentThread().getContextClassLoader().getResourceAsStream(resourceName); + } - public static String printStateReport(RoboContext ctx) { - StringBuilder builder = new StringBuilder(); - List> references = new ArrayList<>(ctx.getUnits()); - references.sort(ID_COMPARATOR); - // formatter:off - builder.append("RoboSystem state ").append(ctx.getState().getLocalizedName()).append(BREAK) - .append(DELIMITER_HORIZONTAL).append(BREAK); - for (RoboReference reference : references) { - builder.append( - String.format(" %-25s %13s", reference.getId(), reference.getState().getLocalizedName())) - .append(BREAK); - } - // formatter:on - return builder.toString(); - } + public static String printStateReport(RoboContext ctx) { + StringBuilder builder = new StringBuilder(); + List> references = new ArrayList<>(ctx.getUnits()); + references.sort(ID_COMPARATOR); + // formatter:off + builder.append("RoboSystem state ").append(ctx.getState().getLocalizedName()).append(BREAK) + .append(DELIMITER_HORIZONTAL).append(BREAK); + for (RoboReference reference : references) { + builder.append( + String.format(" %-25s %13s", reference.getId(), reference.getState().getLocalizedName())) + .append(BREAK); + } + // formatter:on + return builder.toString(); + } - // TODO: 1/25/18 (miro) convert it to JSON message - public static String printSocketEndPoint(RoboReference point, RoboReference codecUnit) { - final int port = point.getConfiguration().getInteger("port", 0); - StringBuilder sb = new StringBuilder(); - //@formatter:off + // TODO: 1/25/18 (miro) convert it to JSON message + public static String printSocketEndPoint(RoboReference point, RoboReference codecUnit) { + final int port = point.getConfiguration().getInteger("port", 0); + StringBuilder sb = new StringBuilder(); + //@formatter:off sb.append("RoboSystem end-points:") .append(BREAK) .append(DELIMITER_HORIZONTAL) @@ -89,21 +91,23 @@ public static String printSocketEndPoint(RoboReference point, RoboReference metadata = new HashMap<>(); - String id = "MyID"; - int heartBeatInterval = 1234; - metadata.put("name", "Pretty Human Readable Name"); - metadata.put("uri", "robo4j://localhost:12345"); - RoboContextDescriptor descriptor = new RoboContextDescriptor(id, heartBeatInterval, metadata); - byte[] encodedDescriptor = HearbeatMessageCodec.encode(descriptor); - RoboContextDescriptor decodedDescriptor = HearbeatMessageCodec.decode(encodedDescriptor); + @Test + void testEncodeDecode() throws IOException { + Map metadata = new HashMap<>(); + String id = "MyID"; + int heartBeatInterval = 1234; + metadata.put("name", "Pretty Human Readable Name"); + metadata.put("uri", "robo4j://localhost:12345"); + RoboContextDescriptor descriptor = new RoboContextDescriptor(id, heartBeatInterval, metadata); + byte[] encodedDescriptor = HearbeatMessageCodec.encode(descriptor); + RoboContextDescriptor decodedDescriptor = HearbeatMessageCodec.decode(encodedDescriptor); - assertEquals(descriptor.getId(), decodedDescriptor.getId()); - assertEquals(descriptor.getHeartBeatInterval(), decodedDescriptor.getHeartBeatInterval()); - assertEquals(descriptor.getMetadata(), decodedDescriptor.getMetadata()); - } + assertEquals(descriptor.getId(), decodedDescriptor.getId()); + assertEquals(descriptor.getHeartBeatInterval(), decodedDescriptor.getHeartBeatInterval()); + assertEquals(descriptor.getMetadata(), decodedDescriptor.getMetadata()); + } - @Test - void testLookup() throws IOException, InterruptedException { - final LookupService service = getLookupService(new LocalLookupServiceImpl()); - service.start(); - RoboContextDescriptor descriptor = createRoboContextDescriptor(); - ContextEmitter emitter = new ContextEmitter(descriptor, InetAddress.getByName(LookupServiceProvider.DEFAULT_MULTICAST_ADDRESS), - LookupServiceProvider.DEFAULT_PORT, 250); + @Test + void testLookup() throws IOException, InterruptedException { + final LookupService service = getLookupService(new LocalLookupServiceImpl()); + service.start(); + RoboContextDescriptor descriptor = createRoboContextDescriptor(); + ContextEmitter emitter = new ContextEmitter(descriptor, InetAddress.getByName(LookupServiceProvider.DEFAULT_MULTICAST_ADDRESS), + LookupServiceProvider.DEFAULT_PORT, 250); - for (int i = 0; i < 10; i++) { - emitter.emit(); - Thread.sleep(250); - } - Map discoveredContexts = service.getDiscoveredContexts(); - System.out.println(discoveredContexts); - assertEquals(1, discoveredContexts.size()); - RoboContext context = service.getContext(descriptor.getId()); - assertNotNull(context); - ClientRemoteRoboContext remoteContext = (ClientRemoteRoboContext) context; - assertNotNull(remoteContext.getAddress()); - System.out.println("Address: " + remoteContext.getAddress()); - } + // TODO : review sleep usage + for (int i = 0; i < 10; i++) { + emitter.emit(); + Thread.sleep(250); + } - private static RoboContextDescriptor createRoboContextDescriptor() { - Map metadata = new HashMap<>(); - String id = "MyID"; - int heartBeatInterval = 1234; - metadata.put("name", "Pretty Human Readable Name"); - metadata.put(RoboContextDescriptor.KEY_URI, "robo4j://localhost:12345"); - return new RoboContextDescriptor(id, heartBeatInterval, metadata); - } + Map discoveredContexts = service.getDiscoveredContexts(); + RoboContext context = service.getContext(descriptor.getId()); + ClientRemoteRoboContext remoteContext = (ClientRemoteRoboContext) context; + + LOGGER.info("discoveredContexts:{}", discoveredContexts); + LOGGER.info("Address:{}", remoteContext.getAddress()); + assertNotNull(context); + assertNotNull(remoteContext.getAddress()); + assertEquals(1, discoveredContexts.size()); + } + + private static RoboContextDescriptor createRoboContextDescriptor() { + Map metadata = new HashMap<>(); + String id = "MyID"; + int heartBeatInterval = 1234; + metadata.put("name", "Pretty Human Readable Name"); + metadata.put(RoboContextDescriptor.KEY_URI, "robo4j://localhost:12345"); + return new RoboContextDescriptor(id, heartBeatInterval, metadata); + } } diff --git a/robo4j-core/src/test/java/com/robo4j/net/MessageServerTest.java b/robo4j-core/src/test/java/com/robo4j/net/MessageServerTest.java index 7993492c..0839826f 100644 --- a/robo4j-core/src/test/java/com/robo4j/net/MessageServerTest.java +++ b/robo4j-core/src/test/java/com/robo4j/net/MessageServerTest.java @@ -16,11 +16,12 @@ */ package com.robo4j.net; -import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; import com.robo4j.configuration.ConfigurationFactory; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.ArrayList; @@ -34,172 +35,171 @@ import static org.junit.jupiter.api.Assertions.*; /** - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ // TODO : remove thread sleep public class MessageServerTest { - private static final String CONST_MYUUID = "myuuid"; - private static final String PROPERTY_SERVER_NAME = "ServerName"; - private volatile Exception exception = null; - - @Test - void testClientServerMessagePassing() throws Exception { - final List messages = new ArrayList<>(); - final CountDownLatch messageLatch = new CountDownLatch(3); - - Configuration serverConfig = new ConfigurationBuilder().addString(PROPERTY_SERVER_NAME, "Server Name") - .addString(MessageServer.KEY_HOST_NAME, "localhost").build(); - MessageServer server = new MessageServer((uuid, id, message) -> { - System.out.println("Got uuid: " + uuid + " id:" + id + " message:" + message); - messages.add(String.valueOf(message)); - messageLatch.countDown(); - }, serverConfig); - - Thread t = new Thread(() -> { - try { - server.start(); - } catch (IOException e) { - exception = e; - fail(e.getMessage()); - } - }, "Server Listener"); - t.setDaemon(true); - t.start(); - for (int i = 0; i < 10; i++) { - if (server.getListeningPort() == 0) { - Thread.sleep(250); - } else { - break; - } - } - - Configuration clientConfig = ConfigurationFactory.createEmptyConfiguration(); - MessageClient client = new MessageClient(server.getListeningURI(), CONST_MYUUID, clientConfig); - if (exception != null) { - throw exception; - } - - List testMessage = getOrderedTestMessage("My First Little Message!", "My Second Little Message!", - "My Third Little Message!"); - client.connect(); - for (String message : testMessage) { - client.sendMessage("test", message); - } - - var receivedMessages =messageLatch.await(2, TimeUnit.SECONDS); - assertTrue(receivedMessages); - assertEquals(testMessage.size(), messages.size()); - assertArrayEquals(testMessage.toArray(), messages.toArray()); - } - - private List getOrderedTestMessage(String... messages) { - if (messages == null || messages.length == 0) { - fail("Expected message"); - } - return Stream.of(messages).collect(Collectors.toCollection(LinkedList::new)); - } - - @Test - void testMessageTypes() throws Exception { - final String messageText = "Lalala"; - final int messagesNumber = 8; - final List messages = new ArrayList<>(messagesNumber); - final CountDownLatch messageLatch = new CountDownLatch(messagesNumber); - - Configuration serverConfig = new ConfigurationBuilder().addString(PROPERTY_SERVER_NAME, "Server Name") - .addString(MessageServer.KEY_HOST_NAME, "localhost").build(); - MessageServer server = new MessageServer((uuid, id, message) -> { - System.out.println("Got uuid: " + uuid + " got id:" + id + " message:" + message); - messages.add(message); - messageLatch.countDown(); - }, serverConfig); - - Thread t = new Thread(() -> { - try { - server.start(); - } catch (IOException e) { - exception = e; - fail(e.getMessage()); - } - }, "Server Listener"); - t.setDaemon(true); - t.start(); - for (int i = 0; i < 10; i++) { - if (server.getListeningPort() == 0) { - Thread.sleep(250); - } else { - break; - } - } - - Configuration clientConfig = ConfigurationFactory.createEmptyConfiguration(); - MessageClient client = new MessageClient(server.getListeningURI(), CONST_MYUUID, clientConfig); - if (exception != null) { - throw exception; - } - client.connect(); - - client.sendMessage("test1", Byte.valueOf((byte) 1)); - client.sendMessage("test2", Short.valueOf((short) 2)); - client.sendMessage("test3", Character.valueOf((char) 3)); - client.sendMessage("test4", Integer.valueOf(4)); - client.sendMessage("test5", Float.valueOf(5.0f)); - client.sendMessage("test6", Long.valueOf(6)); - client.sendMessage("test7", Double.valueOf(7)); - client.sendMessage("test8", new TestMessageType(8, messageText, null)); - messageLatch.await(24, TimeUnit.HOURS); - - assertEquals(messagesNumber, messages.size()); - if (messages.get(0) instanceof Byte) { - assertEquals(((Byte) messages.get(0)).byteValue(), 1); - } else { - fail("Expected Byte!"); - } - if (messages.get(1) instanceof Short) { - assertEquals(((Short) messages.get(1)).shortValue(), 2); - } else { - fail("Expected Short!"); - } - if (messages.get(2) instanceof Character) { - assertEquals(((Character) messages.get(2)).charValue(), 3); - } else { - fail("Expected Character!"); - } - if (messages.get(3) instanceof Integer) { - assertEquals(((Integer) messages.get(3)).intValue(), 4); - } else { - fail("Expected Integer!"); - } - if (messages.get(4) instanceof Float) { - assertEquals(((Float) messages.get(4)).floatValue(), 5.0f, 0.000001); - } else { - fail("Expected Float!"); - } - if (messages.get(5) instanceof Long) { - assertEquals(((Long) messages.get(5)).longValue(), 6); - } else { - fail("Expected Long!"); - } - if (messages.get(6) instanceof Double) { - assertEquals(((Double) messages.get(6)).doubleValue(), 7.0, 0.000001); - } else { - fail("Expected Double!"); - } - if (messages.get(7) instanceof TestMessageType) { - TestMessageType message = (TestMessageType) messages.get(7); - assertEquals(message.getNumber(), 8); - assertEquals(message.getText(), messageText); - } else { - fail("Expected TestMessageType!"); - } - } - - public static RoboContext createTestContext() { - RoboTestContext testContext = new RoboTestContext("TestContext", ConfigurationFactory.createEmptyConfiguration()); - Configuration configuration = new ConfigurationBuilder().addString("name", "Test").addString("description", "Lalalala").build(); - testContext.addRef(new RoboTestReference("test", configuration)); - return testContext; - } + private static final Logger LOGGER = LoggerFactory.getLogger(MessageServerTest.class); + private static final String CONST_MYUUID = "myuuid"; + private static final String PROPERTY_SERVER_NAME = "ServerName"; + private volatile Exception exception = null; + + @Test + void testClientServerMessagePassing() throws Exception { + final List messages = new ArrayList<>(); + final CountDownLatch messageLatch = new CountDownLatch(3); + + Configuration serverConfig = new ConfigurationBuilder().addString(PROPERTY_SERVER_NAME, "Server Name") + .addString(MessageServer.KEY_HOST_NAME, "localhost").build(); + MessageServer server = new MessageServer((uuid, id, message) -> { + printInfo(uuid, id, message); + messages.add(String.valueOf(message)); + messageLatch.countDown(); + }, serverConfig); + + Thread t = new Thread(() -> { + try { + server.start(); + } catch (IOException e) { + exception = e; + fail(e.getMessage()); + } + }, "Server Listener"); + t.setDaemon(true); + t.start(); + for (int i = 0; i < 10; i++) { + if (server.getListeningPort() == 0) { + Thread.sleep(250); + } else { + break; + } + } + + Configuration clientConfig = ConfigurationFactory.createEmptyConfiguration(); + MessageClient client = new MessageClient(server.getListeningURI(), CONST_MYUUID, clientConfig); + if (exception != null) { + throw exception; + } + + List testMessage = getOrderedTestMessage("My First Little Message!", "My Second Little Message!", + "My Third Little Message!"); + client.connect(); + for (String message : testMessage) { + client.sendMessage("test", message); + } + + var receivedMessages = messageLatch.await(2, TimeUnit.SECONDS); + + assertTrue(receivedMessages); + assertEquals(testMessage.size(), messages.size()); + assertArrayEquals(testMessage.toArray(), messages.toArray()); + } + + private List getOrderedTestMessage(String... messages) { + if (messages == null || messages.length == 0) { + fail("Expected message"); + } + return Stream.of(messages).collect(Collectors.toCollection(LinkedList::new)); + } + + @Test + void testMessageTypes() throws Exception { + final String messageText = "Lalala"; + final int messagesNumber = 8; + final List messages = new ArrayList<>(messagesNumber); + final CountDownLatch messageLatch = new CountDownLatch(messagesNumber); + + Configuration serverConfig = new ConfigurationBuilder().addString(PROPERTY_SERVER_NAME, "Server Name") + .addString(MessageServer.KEY_HOST_NAME, "localhost").build(); + MessageServer server = new MessageServer((uuid, id, message) -> { + printInfo(uuid, id, message); + messages.add(message); + messageLatch.countDown(); + }, serverConfig); + + Thread t = new Thread(() -> { + try { + server.start(); + } catch (IOException e) { + exception = e; + fail(e.getMessage()); + } + }, "Server Listener"); + t.setDaemon(true); + t.start(); + for (int i = 0; i < 10; i++) { + if (server.getListeningPort() == 0) { + Thread.sleep(250); + } else { + break; + } + } + + Configuration clientConfig = ConfigurationFactory.createEmptyConfiguration(); + MessageClient client = new MessageClient(server.getListeningURI(), CONST_MYUUID, clientConfig); + if (exception != null) { + throw exception; + } + client.connect(); + + // TODO : review boxing + client.sendMessage("test1", Byte.valueOf((byte) 1)); + client.sendMessage("test2", Short.valueOf((short) 2)); + client.sendMessage("test3", Character.valueOf((char) 3)); + client.sendMessage("test4", Integer.valueOf(4)); + client.sendMessage("test5", Float.valueOf(5.0f)); + client.sendMessage("test6", Long.valueOf(6)); + client.sendMessage("test7", Double.valueOf(7)); + client.sendMessage("test8", new TestMessageType(8, messageText, null)); + messageLatch.await(24, TimeUnit.HOURS); + + assertEquals(messagesNumber, messages.size()); + if (messages.get(0) instanceof Byte) { + assertEquals(((Byte) messages.get(0)).byteValue(), 1); + } else { + fail("Expected Byte!"); + } + if (messages.get(1) instanceof Short) { + assertEquals(((Short) messages.get(1)).shortValue(), 2); + } else { + fail("Expected Short!"); + } + if (messages.get(2) instanceof Character) { + assertEquals(((Character) messages.get(2)).charValue(), 3); + } else { + fail("Expected Character!"); + } + if (messages.get(3) instanceof Integer) { + assertEquals(((Integer) messages.get(3)).intValue(), 4); + } else { + fail("Expected Integer!"); + } + if (messages.get(4) instanceof Float) { + assertEquals(((Float) messages.get(4)).floatValue(), 5.0f, 0.000001); + } else { + fail("Expected Float!"); + } + if (messages.get(5) instanceof Long) { + assertEquals(((Long) messages.get(5)).longValue(), 6); + } else { + fail("Expected Long!"); + } + if (messages.get(6) instanceof Double) { + assertEquals(((Double) messages.get(6)).doubleValue(), 7.0, 0.000001); + } else { + fail("Expected Double!"); + } + if (messages.get(7) instanceof TestMessageType) { + TestMessageType message = (TestMessageType) messages.get(7); + assertEquals(message.getNumber(), 8); + assertEquals(message.getText(), messageText); + } else { + fail("Expected TestMessageType!"); + } + } + + private static void printInfo(String uuid, String id, Object message) { + LOGGER.info("Got uuid: {} got id:{} message:{}", uuid, id, message); + } } diff --git a/robo4j-core/src/test/java/com/robo4j/net/RemoteContextTests.java b/robo4j-core/src/test/java/com/robo4j/net/RemoteContextTests.java index 77ae9735..c05eacd6 100644 --- a/robo4j-core/src/test/java/com/robo4j/net/RemoteContextTests.java +++ b/robo4j-core/src/test/java/com/robo4j/net/RemoteContextTests.java @@ -16,26 +16,22 @@ */ package com.robo4j.net; -import com.robo4j.ConfigurationException; -import com.robo4j.RoboBuilder; -import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; -import com.robo4j.RoboReference; -import com.robo4j.units.StringConsumer; -import com.robo4j.units.StringProducerRemote; +import com.robo4j.*; import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; +import com.robo4j.units.StringConsumer; +import com.robo4j.units.StringProducerRemote; import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.List; import java.util.concurrent.CountDownLatch; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * Note that on Mac OS X, it seems the easiest way to get this test to run is to @@ -46,278 +42,282 @@ */ class RemoteContextTests { - private static final String ACK_CONSUMER = "ackConsumer"; - private static final String REMOTE_UNIT_EMITTER = "remoteEmitter"; - private static final int NUMBER_ITERATIONS = 10; - private static final String REMOTE_CONTEXT_RECEIVER = "remoteReceiver"; - private static final String UNIT_STRING_CONSUMER = "stringConsumer"; - private static final String CONTEXT_ID_REMOTE_RECEIVER = "remoteReceiver"; - - @Test - void discoveryOfDiscoveryEnabledRoboContextTest() throws RoboBuilderException, IOException { - RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testDiscoverableSystem.xml")); - RoboContext ctx = builder.build(); - ctx.start(); - - final LookupService service = LookupServiceTests.getLookupService(new LocalLookupServiceImpl()); - - service.start(); - for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("6") == null); i++) { - SystemUtil.sleep(200); - } - - assertTrue(service.getDiscoveredContexts().size() > 0); - RoboContextDescriptor descriptor = service.getDescriptor("6"); - assertEquals(descriptor.getMetadata().get("name"), "Caprica"); - assertEquals(descriptor.getMetadata().get("class"), "Cylon"); - ctx.shutdown(); - } - - @Test - void messageToDiscoveredContextTest() throws RoboBuilderException, IOException, ConfigurationException { - RoboBuilder builder = new RoboBuilder( - SystemUtil.getInputStreamByResourceName("testRemoteMessageReceiverSystem.xml")); - StringConsumer consumer = new StringConsumer(builder.getContext(), ACK_CONSUMER); - builder.add(consumer); - RoboContext receiverCtx = builder.build(); - receiverCtx.start(); - - // Note that all this cludging about with local lookup service - // implementations etc would normally not be needed. - // This is just to isolate this test from other tests. - final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); - final LookupService service = LookupServiceTests.getLookupService(localLookup); - - LookupServiceProvider.setDefaultLookupService(service); - service.start(); - - for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("7") == null); i++) { - SystemUtil.sleep(200); - } - assertTrue(service.getDiscoveredContexts().size() > 0); - RoboContextDescriptor descriptor = service.getDescriptor("7"); - assertNotNull(descriptor); - - builder = new RoboBuilder( - RemoteContextTests.class.getClassLoader().getResourceAsStream("testMessageEmitterSystem_10.xml")); - RemoteStringProducer remoteStringProducer = new RemoteStringProducer(builder.getContext(), REMOTE_UNIT_EMITTER); - remoteStringProducer.initialize(getEmitterConfiguration("7", ACK_CONSUMER)); - builder.add(remoteStringProducer); - RoboContext emitterContext = builder.build(); - localLookup.addContext(emitterContext); - - emitterContext.start(); - - remoteStringProducer.sendMessage("sendRandomMessage"); - for (int i = 0; i < NUMBER_ITERATIONS && consumer.getReceivedMessages().size() == 0; i++) { - SystemUtil.sleep(200); - } - - assertTrue(consumer.getReceivedMessages().size() > 0); - System.out.println("Got messages: " + consumer.getReceivedMessages()); - emitterContext.shutdown(); - receiverCtx.shutdown(); - } - - @Test - void messageIncludingReferenceToDiscoveredContextTest() - throws RoboBuilderException, IOException, ConfigurationException { - RoboBuilder builder = new RoboBuilder( - SystemUtil.getInputStreamByResourceName("testRemoteMessageReceiverAckSystem.xml")); - AckingStringConsumer consumer = new AckingStringConsumer(builder.getContext(), ACK_CONSUMER); - builder.add(consumer); - RoboContext receiverCtx = builder.build(); - receiverCtx.start(); - - final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); - final LookupService service = LookupServiceTests.getLookupService(localLookup); - - LookupServiceProvider.setDefaultLookupService(service); - service.start(); - - for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("9") == null); i++) { - SystemUtil.sleep(200); - } - assertTrue(service.getDiscoveredContexts().size() > 0); - RoboContextDescriptor descriptor = service.getDescriptor("9"); - assertNotNull(descriptor); - - builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testMessageEmitterSystem_8.xml")); - RemoteTestMessageProducer remoteTestMessageProducer = new RemoteTestMessageProducer(builder.getContext(), - REMOTE_UNIT_EMITTER); - remoteTestMessageProducer.initialize(getEmitterConfiguration("9", ACK_CONSUMER)); - builder.add(remoteTestMessageProducer); - RoboContext emitterContext = builder.build(); - localLookup.addContext(emitterContext); - - emitterContext.start(); - - remoteTestMessageProducer.sendMessage("sendMessage"); - for (int i = 0; i < NUMBER_ITERATIONS && consumer.getReceivedMessages().size() == 0; i++) { - SystemUtil.sleep(200); - } - - assertTrue(consumer.getReceivedMessages().size() > 0); - System.out.println("Got messages: " + consumer.getReceivedMessages()); - - assertTrue(remoteTestMessageProducer.getAckCount() > 0); - } - - @Test - @SuppressWarnings("unchecked") - void messageToDiscoveredContextAndReferenceToDiscoveredContextTest() throws Exception { - RoboContext receiverSystem = buildRemoteReceiverContext(ACK_CONSUMER); - receiverSystem.start(); - RoboReference ackConsumer = receiverSystem.getReference(ACK_CONSUMER); - - // Note that all this cludging about with local lookup service - // implementations etc would normally not be needed. - // This is just to isolate this test from other tests. - final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); - final LookupService service = LookupServiceTests.getLookupService(localLookup); - - LookupServiceProvider.setDefaultLookupService(service); - service.start(); - - // context has been discovered - RoboContextDescriptor contextDescriptor = getRoboContextDescriptor(service, CONTEXT_ID_REMOTE_RECEIVER); - assertTrue(service.getDiscoveredContexts().size() > 0); - assertNotNull(contextDescriptor); - - // build the producer system - RoboContext producerEmitterSystem = buildEmitterContext(TestMessageType.class, ACK_CONSUMER, - REMOTE_UNIT_EMITTER); - producerEmitterSystem.start(); - localLookup.addContext(producerEmitterSystem); - - RoboReference remoteTestMessageProducer = producerEmitterSystem.getReference(REMOTE_UNIT_EMITTER); - remoteTestMessageProducer.sendMessage(RemoteTestMessageProducer.ATTR_SEND_MESSAGE); - CountDownLatch ackConsumerCountDownLatch = ackConsumer - .getAttribute(AckingStringConsumer.DESCRIPTOR_ACK_LATCH).get(); - ackConsumerCountDownLatch.await(); - - List receivedMessages = (List) ackConsumer - .getAttribute(AckingStringConsumer.DESCRIPTOR_MESSAGES).get(); - assertTrue(receivedMessages.size() > 0); - - CountDownLatch producerCountDownLatch = remoteTestMessageProducer - .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_COUNT_DOWN_LATCH).get(); - producerCountDownLatch.await(); - CountDownLatch producerAckLatch = remoteTestMessageProducer - .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_ACK_LATCH).get(); - producerAckLatch.await(); - Integer producerAcknowledge = remoteTestMessageProducer - .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_TOTAL_ACK).get(); - assertTrue(producerAcknowledge > 0); - } - - @Disabled("for individual testing") - @Test - void startRemoteReceiverTest() throws Exception { - buildReceiverSystemStringConsumer(); - - final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); - final LookupService service = LookupServiceTests.getLookupService(localLookup); - - LookupServiceProvider.setDefaultLookupService(service); - service.start(); - System.in.read(); - } - - @Disabled("for individual testing") - @Test - void startRemoteSenderTest() throws Exception { - // Note that all this cludging about with local lookup service - // implementations etc would normally not be needed. - // This is just to isolate this test from other tests. - final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); - final LookupService service = LookupServiceTests.getLookupService(localLookup); - - LookupServiceProvider.setDefaultLookupService(service); - service.start(); - - // context has been discovered - RoboContextDescriptor descriptor = getRoboContextDescriptor(service, CONTEXT_ID_REMOTE_RECEIVER); - assertTrue(service.getDiscoveredContexts().size() > 0); - assertNotNull(descriptor); - - // build the producer system - RoboContext producerEmitterSystem = buildEmitterContext(String.class, UNIT_STRING_CONSUMER, - REMOTE_UNIT_EMITTER); - localLookup.addContext(producerEmitterSystem); - - producerEmitterSystem.start(); - RoboReference remoteTestMessageProducer = producerEmitterSystem.getReference(REMOTE_UNIT_EMITTER); - - for (int i = 0; i < 10; i++) { - remoteTestMessageProducer.sendMessage("REMOTE MESSAGE :" + i); - } - - System.in.read(); - } - - private RoboContext buildEmitterContext(Class clazz, String target, String unitName) throws Exception { - RoboBuilder builder = new RoboBuilder( - SystemUtil.getInputStreamByResourceName("testMessageEmitterSystem_8.xml")); - - if (clazz.equals(String.class)) { - StringProducerRemote remoteTestMessageProducer = new StringProducerRemote<>(clazz, builder.getContext(), - unitName); - remoteTestMessageProducer.initialize(getEmitterConfiguration(REMOTE_CONTEXT_RECEIVER, target)); - builder.add(remoteTestMessageProducer); - } - if (clazz.equals(TestMessageType.class)) { - RemoteTestMessageProducer remoteTestMessageProducer = new RemoteTestMessageProducer(builder.getContext(), - unitName); - remoteTestMessageProducer.initialize(getEmitterConfiguration(REMOTE_CONTEXT_RECEIVER, target)); - builder.add(remoteTestMessageProducer); - } - return builder.build(); - } - - /* - * Builds a system ready to receive TestMessageType messages, which contains a - * reference to which the String message "acknowledge" will be sent when a - * message is received. - */ - private RoboContext buildRemoteReceiverContext(String name) throws RoboBuilderException, ConfigurationException { - RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testRemoteReceiver.xml")); - Configuration configuration = new ConfigurationBuilder() - .addInteger(AckingStringConsumer.ATTR_TOTAL_NUMBER_MESSAGES, 1).build(); - builder.add(AckingStringConsumer.class, configuration, name); - return builder.build(); - } - - /* - * Builds a system ready to receive strings. - */ - private RoboContext buildReceiverSystemStringConsumer() throws RoboBuilderException, ConfigurationException { - RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testRemoteReceiver.xml")); - Configuration configuration = new ConfigurationBuilder().addInteger("totalNumberMessages", 1).build(); - StringConsumer stringConsumer = new StringConsumer(builder.getContext(), UNIT_STRING_CONSUMER); - stringConsumer.initialize(configuration); - - builder.add(stringConsumer); - RoboContext receiverSystem = builder.build(); - receiverSystem.start(); - return receiverSystem; - } - - private RoboContextDescriptor getRoboContextDescriptor(LookupService service, String remoteContextId) { - while (service.getDescriptor(remoteContextId) == null) { - service.getDiscoveredContexts(); - } - RoboContextDescriptor descriptor = service.getDescriptor(remoteContextId); - if (descriptor == null) { - throw new IllegalStateException("not allowed"); - } - return descriptor; - } - - private Configuration getEmitterConfiguration(String targetContext, String target) { - Configuration configuration = new ConfigurationBuilder().addString("target", target) - .addString("targetContext", targetContext).addInteger("totalNumberMessages", 1).build(); - return configuration; - } - + private static final Logger LOGGER = LoggerFactory.getLogger(RemoteContextTests.class); + private static final String ACK_CONSUMER = "ackConsumer"; + private static final String REMOTE_UNIT_EMITTER = "remoteEmitter"; + private static final int NUMBER_ITERATIONS = 10; + private static final String REMOTE_CONTEXT_RECEIVER = "remoteReceiver"; + private static final String UNIT_STRING_CONSUMER = "stringConsumer"; + private static final String CONTEXT_ID_REMOTE_RECEIVER = "remoteReceiver"; + + @Test + void discoveryOfDiscoveryEnabledRoboContextTest() throws RoboBuilderException, IOException { + RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testDiscoverableSystem.xml")); + RoboContext ctx = builder.build(); + ctx.start(); + + final LookupService service = LookupServiceTests.getLookupService(new LocalLookupServiceImpl()); + + service.start(); + for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("6") == null); i++) { + SystemUtil.sleep(200); + } + RoboContextDescriptor descriptor = service.getDescriptor("6"); + + assertFalse(service.getDiscoveredContexts().isEmpty()); + assertEquals(descriptor.getMetadata().get("name"), "Caprica"); + assertEquals(descriptor.getMetadata().get("class"), "Cylon"); + ctx.shutdown(); + } + + // TODO : review a test structure, thread sleep + @Test + void messageToDiscoveredContextTest() throws RoboBuilderException, IOException, ConfigurationException { + RoboBuilder builder = new RoboBuilder( + SystemUtil.getInputStreamByResourceName("testRemoteMessageReceiverSystem.xml")); + StringConsumer consumer = new StringConsumer(builder.getContext(), ACK_CONSUMER); + builder.add(consumer); + RoboContext receiverCtx = builder.build(); + receiverCtx.start(); + + // Note that all this cludging about with local lookup service + // implementations etc would normally not be needed. + // This is just to isolate this test from other tests. + final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); + final LookupService service = LookupServiceTests.getLookupService(localLookup); + + LookupServiceProvider.setDefaultLookupService(service); + service.start(); + + for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("7") == null); i++) { + SystemUtil.sleep(200); + } + assertFalse(service.getDiscoveredContexts().isEmpty()); + RoboContextDescriptor descriptor = service.getDescriptor("7"); + assertNotNull(descriptor); + + builder = new RoboBuilder( + RemoteContextTests.class.getClassLoader().getResourceAsStream("testMessageEmitterSystem_10.xml")); + RemoteStringProducer remoteStringProducer = new RemoteStringProducer(builder.getContext(), REMOTE_UNIT_EMITTER); + remoteStringProducer.initialize(getEmitterConfiguration("7", ACK_CONSUMER)); + builder.add(remoteStringProducer); + RoboContext emitterContext = builder.build(); + localLookup.addContext(emitterContext); + + emitterContext.start(); + + remoteStringProducer.sendMessage("sendRandomMessage"); + for (int i = 0; i < NUMBER_ITERATIONS && consumer.getReceivedMessages().isEmpty(); i++) { + SystemUtil.sleep(200); + } + + printMessagesInfo(consumer.getReceivedMessages()); + assertFalse(consumer.getReceivedMessages().isEmpty()); + emitterContext.shutdown(); + receiverCtx.shutdown(); + } + + @Test + void messageIncludingReferenceToDiscoveredContextTest() + throws RoboBuilderException, IOException, ConfigurationException { + RoboBuilder builder = new RoboBuilder( + SystemUtil.getInputStreamByResourceName("testRemoteMessageReceiverAckSystem.xml")); + AckingStringConsumer consumer = new AckingStringConsumer(builder.getContext(), ACK_CONSUMER); + builder.add(consumer); + RoboContext receiverCtx = builder.build(); + receiverCtx.start(); + + final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); + final LookupService service = LookupServiceTests.getLookupService(localLookup); + + LookupServiceProvider.setDefaultLookupService(service); + service.start(); + + for (int i = 0; i < NUMBER_ITERATIONS && (service.getDescriptor("9") == null); i++) { + SystemUtil.sleep(200); + } + assertFalse(service.getDiscoveredContexts().isEmpty()); + RoboContextDescriptor descriptor = service.getDescriptor("9"); + assertNotNull(descriptor); + + builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testMessageEmitterSystem_8.xml")); + RemoteTestMessageProducer remoteTestMessageProducer = new RemoteTestMessageProducer(builder.getContext(), + REMOTE_UNIT_EMITTER); + remoteTestMessageProducer.initialize(getEmitterConfiguration("9", ACK_CONSUMER)); + builder.add(remoteTestMessageProducer); + RoboContext emitterContext = builder.build(); + localLookup.addContext(emitterContext); + + emitterContext.start(); + + remoteTestMessageProducer.sendMessage("sendMessage"); + for (int i = 0; i < NUMBER_ITERATIONS && consumer.getReceivedMessages().isEmpty(); i++) { + SystemUtil.sleep(200); + } + + + printMessagesInfo(consumer.getReceivedMessages()); + + assertFalse(consumer.getReceivedMessages().isEmpty()); + assertTrue(remoteTestMessageProducer.getAckCount() > 0); + } + + @Test + @SuppressWarnings("unchecked") + void messageToDiscoveredContextAndReferenceToDiscoveredContextTest() throws Exception { + RoboContext receiverSystem = buildRemoteReceiverContext(ACK_CONSUMER); + receiverSystem.start(); + RoboReference ackConsumer = receiverSystem.getReference(ACK_CONSUMER); + + // Note that all this cludging about with local lookup service + // implementations etc would normally not be needed. + // This is just to isolate this test from other tests. + final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); + final LookupService service = LookupServiceTests.getLookupService(localLookup); + + LookupServiceProvider.setDefaultLookupService(service); + service.start(); + + // context has been discovered + RoboContextDescriptor contextDescriptor = getRoboContextDescriptor(service, CONTEXT_ID_REMOTE_RECEIVER); + assertFalse(service.getDiscoveredContexts().isEmpty()); + assertNotNull(contextDescriptor); + + // build the producer system + RoboContext producerEmitterSystem = buildEmitterContext(TestMessageType.class, ACK_CONSUMER, + REMOTE_UNIT_EMITTER); + producerEmitterSystem.start(); + localLookup.addContext(producerEmitterSystem); + + RoboReference remoteTestMessageProducer = producerEmitterSystem.getReference(REMOTE_UNIT_EMITTER); + remoteTestMessageProducer.sendMessage(RemoteTestMessageProducer.ATTR_SEND_MESSAGE); + CountDownLatch ackConsumerCountDownLatch = ackConsumer + .getAttribute(AckingStringConsumer.DESCRIPTOR_ACK_LATCH).get(); + ackConsumerCountDownLatch.await(); + + List receivedMessages = (List) ackConsumer + .getAttribute(AckingStringConsumer.DESCRIPTOR_MESSAGES).get(); + assertFalse(receivedMessages.isEmpty()); + + CountDownLatch producerCountDownLatch = remoteTestMessageProducer + .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_COUNT_DOWN_LATCH).get(); + producerCountDownLatch.await(); + CountDownLatch producerAckLatch = remoteTestMessageProducer + .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_ACK_LATCH).get(); + producerAckLatch.await(); + Integer producerAcknowledge = remoteTestMessageProducer + .getAttribute(RemoteTestMessageProducer.DESCRIPTOR_TOTAL_ACK).get(); + assertTrue(producerAcknowledge > 0); + } + + @Disabled("for individual testing") + @Test + void startRemoteReceiverTest() throws Exception { + buildReceiverSystemStringConsumer(); + + final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); + final LookupService service = LookupServiceTests.getLookupService(localLookup); + + LookupServiceProvider.setDefaultLookupService(service); + service.start(); + System.in.read(); + } + + @Disabled("for individual testing") + @Test + void startRemoteSenderTest() throws Exception { + // Note that all this cludging about with local lookup service + // implementations etc would normally not be needed. + // This is just to isolate this test from other tests. + final LocalLookupServiceImpl localLookup = new LocalLookupServiceImpl(); + final LookupService service = LookupServiceTests.getLookupService(localLookup); + + LookupServiceProvider.setDefaultLookupService(service); + service.start(); + + // context has been discovered + RoboContextDescriptor descriptor = getRoboContextDescriptor(service, CONTEXT_ID_REMOTE_RECEIVER); + assertFalse(service.getDiscoveredContexts().isEmpty()); + assertNotNull(descriptor); + + // build the producer system + RoboContext producerEmitterSystem = buildEmitterContext(String.class, UNIT_STRING_CONSUMER, + REMOTE_UNIT_EMITTER); + localLookup.addContext(producerEmitterSystem); + + producerEmitterSystem.start(); + RoboReference remoteTestMessageProducer = producerEmitterSystem.getReference(REMOTE_UNIT_EMITTER); + + for (int i = 0; i < 10; i++) { + remoteTestMessageProducer.sendMessage("REMOTE MESSAGE :" + i); + } + + System.in.read(); + } + + private RoboContext buildEmitterContext(Class clazz, String target, String unitName) throws Exception { + RoboBuilder builder = new RoboBuilder( + SystemUtil.getInputStreamByResourceName("testMessageEmitterSystem_8.xml")); + + if (clazz.equals(String.class)) { + StringProducerRemote remoteTestMessageProducer = new StringProducerRemote<>(clazz, builder.getContext(), + unitName); + remoteTestMessageProducer.initialize(getEmitterConfiguration(REMOTE_CONTEXT_RECEIVER, target)); + builder.add(remoteTestMessageProducer); + } + if (clazz.equals(TestMessageType.class)) { + RemoteTestMessageProducer remoteTestMessageProducer = new RemoteTestMessageProducer(builder.getContext(), + unitName); + remoteTestMessageProducer.initialize(getEmitterConfiguration(REMOTE_CONTEXT_RECEIVER, target)); + builder.add(remoteTestMessageProducer); + } + return builder.build(); + } + + /* + * Builds a system ready to receive TestMessageType messages, which contains a + * reference to which the String message "acknowledge" will be sent when a + * message is received. + */ + private RoboContext buildRemoteReceiverContext(String name) throws RoboBuilderException, ConfigurationException { + RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testRemoteReceiver.xml")); + Configuration configuration = new ConfigurationBuilder() + .addInteger(AckingStringConsumer.ATTR_TOTAL_NUMBER_MESSAGES, 1).build(); + builder.add(AckingStringConsumer.class, configuration, name); + return builder.build(); + } + + /* + * Builds a system ready to receive strings. + */ + private void buildReceiverSystemStringConsumer() throws RoboBuilderException, ConfigurationException { + RoboBuilder builder = new RoboBuilder(SystemUtil.getInputStreamByResourceName("testRemoteReceiver.xml")); + Configuration configuration = new ConfigurationBuilder().addInteger("totalNumberMessages", 1).build(); + StringConsumer stringConsumer = new StringConsumer(builder.getContext(), UNIT_STRING_CONSUMER); + stringConsumer.initialize(configuration); + + builder.add(stringConsumer); + RoboContext receiverSystem = builder.build(); + receiverSystem.start(); + } + + private RoboContextDescriptor getRoboContextDescriptor(LookupService service, String remoteContextId) { + while (service.getDescriptor(remoteContextId) == null) { + service.getDiscoveredContexts(); + } + RoboContextDescriptor descriptor = service.getDescriptor(remoteContextId); + if (descriptor == null) { + throw new IllegalStateException("not allowed"); + } + return descriptor; + } + + private Configuration getEmitterConfiguration(String targetContext, String target) { + return new ConfigurationBuilder().addString("target", target) + .addString("targetContext", targetContext).addInteger("totalNumberMessages", 1).build(); + } + + private static void printMessagesInfo(List messages) { + LOGGER.info("Got messages: {}", messages); + } } diff --git a/robo4j-core/src/test/java/com/robo4j/net/RemoteStringProducer.java b/robo4j-core/src/test/java/com/robo4j/net/RemoteStringProducer.java index 513f5f69..74ed3957 100644 --- a/robo4j-core/src/test/java/com/robo4j/net/RemoteStringProducer.java +++ b/robo4j-core/src/test/java/com/robo4j/net/RemoteStringProducer.java @@ -16,30 +16,25 @@ */ package com.robo4j.net; -import java.util.concurrent.atomic.AtomicInteger; - -import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; -import com.robo4j.RoboContext; -import com.robo4j.RoboUnit; -import com.robo4j.StringToolkit; +import com.robo4j.*; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.concurrent.atomic.AtomicInteger; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class RemoteStringProducer extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(RemoteStringProducer.class); /* default sent messages */ private static final int DEFAULT = 0; private AtomicInteger counter; private String target; - private String targetContext; + private String targetContext; - /** - * @param context - * @param id - */ public RemoteStringProducer(RoboContext context, String id) { super(String.class, context, id); } @@ -52,7 +47,7 @@ protected void onInitialization(Configuration configuration) throws Configuratio } targetContext = configuration.getString("targetContext", null); if (targetContext == null) { - throw ConfigurationException.createMissingConfigNameException("targetContext"); + throw ConfigurationException.createMissingConfigNameException("targetContext"); } counter = new AtomicInteger(DEFAULT); } @@ -60,7 +55,7 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void onMessage(String message) { if (message == null) { - System.out.println("No Message!"); + LOGGER.info("No Message!"); } else { counter.incrementAndGet(); String[] input = message.split("::"); @@ -70,7 +65,7 @@ public void onMessage(String message) { sendRandomMessage(); break; default: - System.out.println("don't understand message: " + message); + LOGGER.info("don't understand message: {}", message); } } diff --git a/robo4j-core/src/test/java/com/robo4j/net/RemoteTestMessageProducer.java b/robo4j-core/src/test/java/com/robo4j/net/RemoteTestMessageProducer.java index e08b22ce..b010a6df 100644 --- a/robo4j-core/src/test/java/com/robo4j/net/RemoteTestMessageProducer.java +++ b/robo4j-core/src/test/java/com/robo4j/net/RemoteTestMessageProducer.java @@ -16,14 +16,10 @@ */ package com.robo4j.net; -import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; -import com.robo4j.DefaultAttributeDescriptor; -import com.robo4j.RoboContext; -import com.robo4j.RoboUnit; -import com.robo4j.StringToolkit; -import com.robo4j.TestToolkit; +import com.robo4j.*; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.CountDownLatch; import java.util.concurrent.atomic.AtomicInteger; @@ -33,7 +29,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class RemoteTestMessageProducer extends RoboUnit { - + private static final Logger LOGGER = LoggerFactory.getLogger(RemoteTestMessageProducer.class); public static final String ATTR_TOTAL_MESSAGES = "getNumberOfSentMessages"; public static final String ATTR_COUNT_DOWN_LATCH = "countDownLatch"; public static final String ATTR_ACK_LATCH = "ackLatch"; @@ -59,8 +55,8 @@ public class RemoteTestMessageProducer extends RoboUnit { /** - * @param context - * @param id + * @param context RoboContext + * @param id unit id */ public RemoteTestMessageProducer(RoboContext context, String id) { super(String.class, context, id); @@ -88,26 +84,26 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void onMessage(String message) { if (message == null) { - System.out.println("No Message!"); + LOGGER.info("No Message!"); } else { String[] input = message.split("::"); String messageType = input[0]; switch (messageType) { case ATTR_SEND_MESSAGE: totalCounter.incrementAndGet(); - if(countDownLatch != null){ + if (countDownLatch != null) { countDownLatch.countDown(); } sendRandomMessage(); break; case ATTR_ACK: - ackCounter.incrementAndGet(); - if(ackLatch != null){ + ackCounter.incrementAndGet(); + if (ackLatch != null) { ackLatch.countDown(); } - break; + break; default: - System.out.println("don't understand message: " + message); + LOGGER.error("don't understand message: {}", message); } } } @@ -126,7 +122,7 @@ public synchronized R onGetAttribute(AttributeDescriptor attribute) { && attribute.getAttributeType() == CountDownLatch.class) { return (R) ackLatch; } - if (attribute.getAttributeName().equals(ATTR_ACK) && attribute.getAttributeType() == Integer.class){ + if (attribute.getAttributeName().equals(ATTR_ACK) && attribute.getAttributeType() == Integer.class) { return (R) Integer.valueOf(ackCounter.get()); } return null; @@ -135,14 +131,14 @@ public synchronized R onGetAttribute(AttributeDescriptor attribute) { public void sendRandomMessage() { final int number = TestToolkit.getRND().nextInt() % 100; final String text = StringToolkit.getRandomMessage(10); - + // We're sending a reference to ourself for getting the acks... final TestMessageType message = new TestMessageType(number, text, getContext().getReference(getId())); RoboContext ctx = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); ctx.getReference(target).sendMessage(message); } - public int getAckCount() { - return ackCounter.get(); - } + public int getAckCount() { + return ackCounter.get(); + } } diff --git a/robo4j-core/src/test/java/com/robo4j/units/ConfigurationConsumer.java b/robo4j-core/src/test/java/com/robo4j/units/ConfigurationConsumer.java index a495a3a9..ded57836 100644 --- a/robo4j-core/src/test/java/com/robo4j/units/ConfigurationConsumer.java +++ b/robo4j-core/src/test/java/com/robo4j/units/ConfigurationConsumer.java @@ -16,61 +16,63 @@ */ package com.robo4j.units; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.atomic.AtomicInteger; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.atomic.AtomicInteger; /** - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class ConfigurationConsumer extends RoboUnit { - private static final int DEFAULT = 0; - private AtomicInteger counter; - private List receivedMessages = new ArrayList<>(); + private static final Logger LOGGER = LoggerFactory.getLogger(ConfigurationConsumer.class); + private static final int DEFAULT = 0; + private final AtomicInteger counter; + private final List receivedMessages = new ArrayList<>(); + + /** + * @param context RoboContext + * @param id unit id + */ + public ConfigurationConsumer(RoboContext context, String id) { + super(String.class, context, id); + this.counter = new AtomicInteger(DEFAULT); + } - /** - * @param context - * @param id - */ - public ConfigurationConsumer(RoboContext context, String id) { - super(String.class, context, id); - this.counter = new AtomicInteger(DEFAULT); - } + public synchronized List getReceivedMessages() { + return receivedMessages; + } - public synchronized List getReceivedMessages() { - return receivedMessages; - } - - @Override - public synchronized void onMessage(String message) { - counter.incrementAndGet(); - receivedMessages.add(message); - } + @Override + public synchronized void onMessage(String message) { + counter.incrementAndGet(); + receivedMessages.add(message); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - System.out.println(configuration); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + LOGGER.info("configuration:{}", configuration); + } - @SuppressWarnings("unchecked") - @Override - public synchronized R onGetAttribute(AttributeDescriptor attribute) { - if (attribute.getAttributeName().equals("getNumberOfSentMessages") && attribute.getAttributeType() == Integer.class) { - return (R) (Integer)counter.get(); - } - if (attribute.getAttributeName().equals("getReceivedMessages") - && attribute.getAttributeType() == ArrayList.class) { - return (R) receivedMessages; - } - return null; - } + @SuppressWarnings("unchecked") + @Override + public synchronized R onGetAttribute(AttributeDescriptor attribute) { + if (attribute.getAttributeName().equals("getNumberOfSentMessages") && attribute.getAttributeType() == Integer.class) { + return (R) (Integer) counter.get(); + } + if (attribute.getAttributeName().equals("getReceivedMessages") + && attribute.getAttributeType() == ArrayList.class) { + return (R) receivedMessages; + } + return null; + } } diff --git a/robo4j-core/src/test/java/com/robo4j/units/StringProducer.java b/robo4j-core/src/test/java/com/robo4j/units/StringProducer.java index 4ca3cf23..cc64d2fe 100644 --- a/robo4j-core/src/test/java/com/robo4j/units/StringProducer.java +++ b/robo4j-core/src/test/java/com/robo4j/units/StringProducer.java @@ -18,6 +18,8 @@ import com.robo4j.*; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.CountDownLatch; import java.util.concurrent.atomic.AtomicInteger; @@ -27,7 +29,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class StringProducer extends RoboUnit { - + private static final Logger LOGGER = LoggerFactory.getLogger(StringProducer.class); public static final String DEFAULT_UNIT_NAME = "producer"; public static final String ATTR_GET_NUMBER_OF_SENT_MESSAGES = "getNumberOfSentMessages"; public static final String ATTR_COUNT_DOWN_LATCH = "countDownLatch"; @@ -70,7 +72,7 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void onMessage(String message) { if (message == null) { - System.out.println("No Message!"); + LOGGER.info("No Message!"); } else { counter.incrementAndGet(); String[] input = message.split("::"); @@ -81,7 +83,7 @@ public void onMessage(String message) { latch.countDown(); break; default: - System.out.println("don't understand message: " + message); + LOGGER.info("don't understand message: {}", message); } } } diff --git a/robo4j-core/src/test/java/com/robo4j/units/StringProducerRemote.java b/robo4j-core/src/test/java/com/robo4j/units/StringProducerRemote.java index e0136eaa..a2a22928 100644 --- a/robo4j-core/src/test/java/com/robo4j/units/StringProducerRemote.java +++ b/robo4j-core/src/test/java/com/robo4j/units/StringProducerRemote.java @@ -21,6 +21,8 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.net.LookupServiceProvider; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.atomic.AtomicInteger; @@ -31,7 +33,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class StringProducerRemote extends RoboUnit { - + private static final Logger LOGGER = LoggerFactory.getLogger(StringProducerRemote.class); private final AtomicInteger totalCounter = new AtomicInteger(0); private String target; private String targetContext; @@ -57,6 +59,6 @@ public void onMessage(T message) { int totalMessages = totalCounter.incrementAndGet(); RoboContext ctx = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); ctx.getReference(target).sendMessage(message); - System.out.printf("class: %s, totalMessages: %d%n",getClass().getSimpleName(), totalMessages); + LOGGER.info("class:{}, totalMessages:{}", getClass().getSimpleName(), totalMessages); } } diff --git a/robo4j-core/src/test/java/com/robo4j/units/StringScheduledEmitter.java b/robo4j-core/src/test/java/com/robo4j/units/StringScheduledEmitter.java index 6c5602f2..7b6b9b32 100644 --- a/robo4j-core/src/test/java/com/robo4j/units/StringScheduledEmitter.java +++ b/robo4j-core/src/test/java/com/robo4j/units/StringScheduledEmitter.java @@ -19,6 +19,8 @@ import com.robo4j.*; import com.robo4j.configuration.Configuration; import com.robo4j.util.StringConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; @@ -31,7 +33,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class StringScheduledEmitter extends RoboUnit { - + private static final Logger LOGGER = LoggerFactory.getLogger(StringScheduledEmitter.class); public static final String DEFAULT_UNIT_NAME = "scheduledEmitter"; public static final String ATTR_GET_NUMBER_OF_SENT_MESSAGES = "getNumberOfSentMessages"; public static final String ATTR_COUNT_DOWN_LATCH = "countDownLatch"; @@ -75,10 +77,10 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void start() { - System.out.println("start scheduler"); + LOGGER.info("start scheduler"); getContext().getScheduler().scheduleAtFixedRate(() -> { var messageForTarget = messages.get(target); - System.out.println("scheduler message: " + messageForTarget + ", target: " + target); + LOGGER.info("scheduler message: {}, target: {}", messageForTarget, target); getContext().getReference(target).sendMessage(messageForTarget); }, initSchedulerDelayMillis, schedulerPeriodMillis, TimeUnit.MILLISECONDS); } From afe22c5d3e86063f27f8bfd3ea0d4c5f491faf60 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Mon, 7 Oct 2024 21:54:24 +0200 Subject: [PATCH 03/13] [69] robo4j-hw-rpi logger update --- .../AccelerometerLSM303Test.java | 73 +- .../AlphanumericDeviceExample.java | 33 +- .../BiColor24BargraphExample.java | 65 +- .../BiColor8x8MatrixExample.java | 64 +- .../BiColor8x8MatrixFaceExample.java | 44 +- .../BiColor8x8MatrixFaceRotationExample.java | 40 +- .../robo4j/hw/rpi/i2c/adafruitlcd/Demo.java | 2 +- .../i2c/adafruitoled/SSD1306DeviceTest.java | 90 +- .../com/robo4j/hw/rpi/i2c/bmp/BMP085Test.java | 5 +- .../com/robo4j/hw/rpi/i2c/gps/GPSTest.java | 40 +- .../hw/rpi/i2c/gyro/CalibratedGyroTest.java | 43 +- .../hw/rpi/i2c/gyro/GyroL3GD20Test.java | 26 +- .../hw/rpi/i2c/lidar/LidarLiteTest.java | 27 +- .../CalibratedMagnetometerExample.java | 81 +- .../MagnetometerLSM303Example.java | 186 +-- .../robo4j/hw/rpi/i2c/pwm/MC33926Example.java | 12 +- .../robo4j/hw/rpi/i2c/pwm/PCA9685Example.java | 13 +- .../robo4j/hw/rpi/i2c/pwm/ServoTester.java | 109 +- .../com/robo4j/hw/rpi/imu/Bno055Example.java | 156 +- .../rpi/imu/Bno080AccelerometerExample.java | 27 +- .../com/robo4j/hw/rpi/imu/Bno080Example.java | 27 +- .../com/robo4j/hw/rpi/lcd/Lcd20x4Example.java | 99 +- .../robo4j/hw/rpi/pad/LF710PadExample.java | 25 +- .../rpi/pwm/PCA9685TruckPlatformExample.java | 84 +- .../robo4j/hw/rpi/pwm/PWMServoExample.java | 32 +- .../rpi/pwm/roboclaw/RoboClawRCTankTest.java | 66 +- .../com/robo4j/hw/rpi/serial/gps/GPSTest.java | 45 +- .../hw/rpi/serial/ydlidar/YDLidarTest.java | 56 +- .../robo4j/hw/rpi/i2c/AbstractI2CDevice.java | 17 +- .../hw/rpi/i2c/CalibratedFloat3DDevice.java | 71 +- .../AccelerometerLSM303Device.java | 257 +-- .../adafruitbackpack/AbstractBackpack.java | 353 ++-- .../adafruitbackpack/BiColor24BarDevice.java | 6 +- .../adafruitlcd/ButtonPressedObserver.java | 177 +- .../hw/rpi/i2c/adafruitlcd/Message.java | 7 +- .../rpi/i2c/adafruitoled/SSD1306Device.java | 495 +++--- .../robo4j/hw/rpi/i2c/bmp/BMP085Device.java | 413 +++-- .../com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java | 254 ++- .../robo4j/hw/rpi/i2c/gps/XA1110Device.java | 19 +- .../hw/rpi/i2c/gyro/CalibratedGyro.java | 109 +- .../MagnetometerLSM303Device.java | 5 +- .../hw/rpi/imu/bno/impl/Bno080SPIDevice.java | 1419 ++++++++--------- .../rpi/imu/bno/shtp/ShtpPacketResponse.java | 10 +- .../robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java | 301 ++-- .../java/com/robo4j/hw/rpi/lcd/Lcd20x4.java | 374 +++-- .../com/robo4j/hw/rpi/lcd/StringUtils.java | 42 +- .../robo4j/hw/rpi/pwm/VehiclePlatform.java | 114 +- .../hw/rpi/pwm/roboclaw/RoboClawRCTank.java | 204 ++- .../com/robo4j/hw/rpi/serial/SerialUtil.java | 209 ++- .../robo4j/hw/rpi/serial/gps/MTK3339GPS.java | 415 +++-- .../hw/rpi/serial/ydlidar/YDLidarDevice.java | 21 +- robo4j-hw-rpi/src/main/java/module-info.java | 1 + .../hw/rpi/i2c/gps/TestEventDecoding.java | 77 +- .../com/robo4j/hw/rpi/i2c/gps/TestGPS.java | 48 +- 54 files changed, 3465 insertions(+), 3523 deletions(-) diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Test.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Test.java index 3b9b5464..c402c10c 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Test.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Test.java @@ -16,52 +16,55 @@ */ package com.robo4j.hw.rpi.i2c.accelerometer; -import java.io.IOException; - import com.robo4j.hw.rpi.i2c.ReadableDevice; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Example useful to check if your accelerometer is working properly. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AccelerometerLSM303Test { - public static void main(String[] args) throws IOException, InterruptedException { - ReadableDevice device = new AccelerometerLSM303Device(); - getReading(device, "Place the device in the position(s) you want to measure"); - } + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerLSM303Test.class); + + public static void main(String[] args) throws IOException, InterruptedException { + ReadableDevice device = new AccelerometerLSM303Device(); + getReading(device, "Place the device in the position(s) you want to measure"); + } - private static void getReading(ReadableDevice device, String message) - throws IOException, InterruptedException { - prompt(message); - print(readValues(device)); - } + private static void getReading(ReadableDevice device, String message) + throws IOException, InterruptedException { + prompt(message); + print(readValues(device)); + } - private static void print(Stats stats) { - System.out.println("Result:"); - System.out.println(stats); - } + private static void print(Stats stats) { + LOGGER.debug("Result:{}", stats); + } - private static void prompt(String msg) throws IOException { - System.out.println(msg); - System.out.println("Press to continue!"); - System.in.read(); - } + private static void prompt(String msg) throws IOException { + LOGGER.debug(msg); + LOGGER.debug("Press to continue!"); + System.in.read(); + } - private static Stats readValues(ReadableDevice device) throws IOException, InterruptedException { - // TODO: change print... - Stats stats = new Stats(); - for (int i = 0; i < 250; i++) { - Tuple3f fl = device.read(); - stats.addValue(fl); - Thread.sleep(20); - if (i % 25 == 0) { - System.out.print("."); - } - } - System.out.println(""); - return stats; - } + private static Stats readValues(ReadableDevice device) throws IOException, InterruptedException { + // TODO: change print... + Stats stats = new Stats(); + for (int i = 0; i < 250; i++) { + Tuple3f fl = device.read(); + stats.addValue(fl); + Thread.sleep(20); + if (i % 25 == 0) { + LOGGER.debug("."); + } + } + LOGGER.debug(""); + return stats; + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AlphanumericDeviceExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AlphanumericDeviceExample.java index 3c65118c..bbea55cc 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AlphanumericDeviceExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AlphanumericDeviceExample.java @@ -17,6 +17,9 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.concurrent.TimeUnit; /** @@ -26,18 +29,20 @@ * @author Miroslav Wengner (@miragemiko) */ public class AlphanumericDeviceExample { - public static void main(String[] args) throws Exception { - System.out.println("=== Alphanumeric Backpack Example ==="); - - AlphanumericDevice device = new AlphanumericDevice(); - device.clear(); - device.display(); - System.out.println("Adding Characters"); - device.addCharacter('A', false); - device.addCharacter('B', true); - device.addCharacter('C', false); - device.addCharacter('D', true); - device.display(); + private static final Logger LOGGER = LoggerFactory.getLogger(AlphanumericDeviceExample.class); + + public static void main(String[] args) throws Exception { + LOGGER.debug("=== Alphanumeric Backpack Example ==="); + + AlphanumericDevice device = new AlphanumericDevice(); + device.clear(); + device.display(); + LOGGER.debug("Adding Characters"); + device.addCharacter('A', false); + device.addCharacter('B', true); + device.addCharacter('C', false); + device.addCharacter('D', true); + device.display(); TimeUnit.SECONDS.sleep(3); device.clear(); device.display(); @@ -48,10 +53,10 @@ public static void main(String[] args) throws Exception { device.display(); - System.out.println("Press to quit!"); + LOGGER.debug("Press to quit!"); System.in.read(); device.clear(); device.display(); - } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BargraphExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BargraphExample.java index e68a38f4..bf663221 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BargraphExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BargraphExample.java @@ -17,51 +17,54 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.concurrent.TimeUnit; /** * Simple example of using Adafruit BiColor Bargraph {@link BiColor24BarDevice} * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class BiColor24BargraphExample { + private static final Logger LOGGER = LoggerFactory.getLogger(BiColor24BargraphExample.class); - public static void main(String[] args) throws Exception { - System.out.println("=== Bi-BiColor 24 Bargraph ==="); + public static void main(String[] args) throws Exception { + LOGGER.debug("=== Bi-BiColor 24 Bargraph ==="); - BiColor24BarDevice device = new BiColor24BarDevice(); - device.clear(); - device.display(); + BiColor24BarDevice device = new BiColor24BarDevice(); + device.clear(); + device.display(); - for (int i = 0; i < device.getMaxBar(); i++) { - device.setBar(i, BiColor.GREEN); - device.display(); - TimeUnit.MILLISECONDS.sleep(200); - } + for (int i = 0; i < device.getMaxBar(); i++) { + device.setBar(i, BiColor.GREEN); + device.display(); + TimeUnit.MILLISECONDS.sleep(200); + } - for (int i = device.getMaxBar() - 1; i >= 0; i--) { - device.setBar(i, BiColor.OFF); - device.display(); - TimeUnit.MILLISECONDS.sleep(100); - } + for (int i = device.getMaxBar() - 1; i >= 0; i--) { + device.setBar(i, BiColor.OFF); + device.display(); + TimeUnit.MILLISECONDS.sleep(100); + } - int counter = 0; - while (counter < 3) { - for (int i = 0; i < 12; i++) { - int colorNumber = (i + counter) % 3 + 1; - device.setBar(i, BiColor.getByValue(colorNumber)); - TimeUnit.MILLISECONDS.sleep(200); - device.display(); - } - counter++; - } + int counter = 0; + while (counter < 3) { + for (int i = 0; i < 12; i++) { + int colorNumber = (i + counter) % 3 + 1; + device.setBar(i, BiColor.getByValue(colorNumber)); + TimeUnit.MILLISECONDS.sleep(200); + device.display(); + } + counter++; + } - System.out.println("Press to quit!"); - System.in.read(); - device.clear(); - device.display(); - } + LOGGER.debug("Press to quit!"); + System.in.read(); + device.clear(); + device.display(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixExample.java index b1136ff7..0747bc8e 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixExample.java @@ -17,6 +17,9 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.concurrent.TimeUnit; /** @@ -26,35 +29,36 @@ * @author Miroslav Wengner (@miragemiko) */ public class BiColor8x8MatrixExample { + private static final Logger LOGGER = LoggerFactory.getLogger(BiColor8x8MatrixExample.class); + + public static void main(String[] args) throws Exception { + LOGGER.debug("=== BiColor 8x8 Matrix Example ==="); + + BiColor8x8MatrixDevice matrix = new BiColor8x8MatrixDevice(); + matrix.clear(); + matrix.display(); + + MatrixRotation[] rotations = {MatrixRotation.DEFAULT_X_Y, MatrixRotation.RIGHT_90, MatrixRotation.RIGHT_180, + MatrixRotation.RIGHT_270, MatrixRotation.LEFT_90}; + for (MatrixRotation rotation : rotations) { + matrix.setRotation(rotation); + matrix.drawPixel(0, 0, BiColor.RED); + matrix.drawPixel(1, 0, BiColor.GREEN); + matrix.drawPixel(2, 0, BiColor.YELLOW); + matrix.drawPixel(3, 0, BiColor.RED); + matrix.drawPixel(0, 1, BiColor.GREEN); + matrix.drawPixel(0, 2, BiColor.YELLOW); + matrix.drawPixel(7, 7, BiColor.GREEN); + matrix.drawPixel(7, 6, BiColor.GREEN); + matrix.display(); + TimeUnit.SECONDS.sleep(1); + matrix.clear(); + } + + LOGGER.debug("Press to quit!"); + System.in.read(); + matrix.clear(); + matrix.display(); - public static void main(String[] args) throws Exception { - System.out.println("=== BiColor 8x8 Matrix Example ==="); - - BiColor8x8MatrixDevice matrix = new BiColor8x8MatrixDevice(); - matrix.clear(); - matrix.display(); - - MatrixRotation[] rotations = { MatrixRotation.DEFAULT_X_Y, MatrixRotation.RIGHT_90, MatrixRotation.RIGHT_180, - MatrixRotation.RIGHT_270, MatrixRotation.LEFT_90 }; - for (MatrixRotation rotation : rotations) { - matrix.setRotation(rotation); - matrix.drawPixel(0, 0, BiColor.RED); - matrix.drawPixel(1, 0, BiColor.GREEN); - matrix.drawPixel(2, 0, BiColor.YELLOW); - matrix.drawPixel(3, 0, BiColor.RED); - matrix.drawPixel(0, 1, BiColor.GREEN); - matrix.drawPixel(0, 2, BiColor.YELLOW); - matrix.drawPixel(7, 7, BiColor.GREEN); - matrix.drawPixel(7, 6, BiColor.GREEN); - matrix.display(); - TimeUnit.SECONDS.sleep(1); - matrix.clear(); - } - - System.out.println("Press to quit!"); - System.in.read(); - matrix.clear(); - matrix.display(); - - } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceExample.java index 257e49d0..6e5baa3d 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceExample.java @@ -17,6 +17,9 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.Arrays; import java.util.List; import java.util.concurrent.TimeUnit; @@ -28,31 +31,32 @@ * @author Miroslav Wengner (@miragemiko) */ public class BiColor8x8MatrixFaceExample { + private static final Logger LOGGER = LoggerFactory.getLogger(BiColor8x8MatrixFaceExample.class); - public static void main(String[] args) throws Exception { - System.out.println("=== BiColor 8x8 Matrix Face Example ==="); + public static void main(String[] args) throws Exception { + LOGGER.debug("=== BiColor 8x8 Matrix Face Example ==="); - BiColor8x8MatrixDevice led = new BiColor8x8MatrixDevice(); + BiColor8x8MatrixDevice led = new BiColor8x8MatrixDevice(); - char[] faceSmile = "00333300,03000030,30300303,30000003,30300303,30033003,03000030,00333300".toCharArray(); - char[] faceNeutral = "00222200,02000020,20200202,20000002,20222202,20000002,02000020,00222200".toCharArray(); - char[] faceSad = "00111100,01000010,10100101,10000001,10011001,10100101,01000010,00111100".toCharArray(); + char[] faceSmile = "00333300,03000030,30300303,30000003,30300303,30033003,03000030,00333300".toCharArray(); + char[] faceNeutral = "00222200,02000020,20200202,20000002,20222202,20000002,02000020,00222200".toCharArray(); + char[] faceSad = "00111100,01000010,10100101,10000001,10011001,10100101,01000010,00111100".toCharArray(); - List availableFaces = Arrays.asList(faceSad, faceNeutral, faceSmile); + List availableFaces = Arrays.asList(faceSad, faceNeutral, faceSmile); - for (char[] face : availableFaces) { - led.clear(); - led.display(); - byte[] faceBytes = LedBackpackUtils.createMatrixBiColorArrayByCharSequence(led.getWidth(), ',', face); - LedBackpackUtils.paintByBiColorByteArray(led, faceBytes); - led.display(); - TimeUnit.SECONDS.sleep(1); - } + for (char[] face : availableFaces) { + led.clear(); + led.display(); + byte[] faceBytes = LedBackpackUtils.createMatrixBiColorArrayByCharSequence(led.getWidth(), ',', face); + LedBackpackUtils.paintByBiColorByteArray(led, faceBytes); + led.display(); + TimeUnit.SECONDS.sleep(1); + } - System.out.println("Press to quit!"); - System.in.read(); - led.clear(); - led.display(); + LOGGER.debug("Press to quit!"); + System.in.read(); + led.clear(); + led.display(); - } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceRotationExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceRotationExample.java index 130f23a3..f5351116 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceRotationExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor8x8MatrixFaceRotationExample.java @@ -17,6 +17,9 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.util.concurrent.TimeUnit; @@ -27,12 +30,13 @@ * @author Miroslav Wengner (@miragemiko) */ public class BiColor8x8MatrixFaceRotationExample { + private static final Logger LOGGER = LoggerFactory.getLogger(BiColor8x8MatrixFaceRotationExample.class); - public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("=== BiColor 8x8 Matrix Face Rotation Example ==="); + public static void main(String[] args) throws IOException, InterruptedException { + LOGGER.debug("=== BiColor 8x8 Matrix Face Rotation Example ==="); - BiColor8x8MatrixDevice ledMatrix = new BiColor8x8MatrixDevice(); - //@formatter:off + BiColor8x8MatrixDevice ledMatrix = new BiColor8x8MatrixDevice(); + //@formatter:off byte[] faceSmile = { 0b0011_1100, 0b0100_0010, @@ -45,18 +49,18 @@ public static void main(String[] args) throws IOException, InterruptedException }; //@formatter:on - for (int i = 0; i < faceSmile.length; i++) { - ledMatrix.clear(); - ledMatrix.display(); - LedBackpackUtils.paintToByRowArraysAndColor(ledMatrix, faceSmile, BiColor.getByValue(i % 2 + 1)); - ledMatrix.display(); - ledMatrix.setRotation(MatrixRotation.getById(i % 5 + 1)); - TimeUnit.SECONDS.sleep(1); - } - - System.out.println("Press to quit!"); - System.in.read(); - ledMatrix.clear(); - ledMatrix.display(); - } + for (int i = 0; i < faceSmile.length; i++) { + ledMatrix.clear(); + ledMatrix.display(); + LedBackpackUtils.paintToByRowArraysAndColor(ledMatrix, faceSmile, BiColor.getByValue(i % 2 + 1)); + ledMatrix.display(); + ledMatrix.setRotation(MatrixRotation.getById(i % 5 + 1)); + TimeUnit.SECONDS.sleep(1); + } + + LOGGER.debug("Press to quit!"); + System.in.read(); + ledMatrix.clear(); + ledMatrix.display(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Demo.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Demo.java index b9282ed0..fe43174e 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Demo.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Demo.java @@ -1 +1 @@ -/* * Copyright (c) 2014, 2024, Marcus Hirt, Miroslav Wengner * * Robo4J is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Robo4J is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Robo4J. If not, see . */ package com.robo4j.hw.rpi.i2c.adafruitlcd; import java.io.IOException; import com.robo4j.hw.rpi.i2c.adafruitlcd.impl.AdafruitLcdImpl.Direction; /** * Here is a demonstration of things that can be done with the LCD shield. Run * this as a standalone java program. Do not forget to have sudo rights. * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Demo { private static final LCDDemo[] TESTS = { new HelloWorldDemo(), new ScrollDemo(), new CursorDemo(), new DisplayDemo(), new ColorDemo(), new AutoScrollDemo(), new ExitDemo() }; private static int currentTest = -1; public static void main(String[] args) throws IOException { final AdafruitLcd lcd = LcdFactory.createLCD(); lcd.setText("LCD Test!\nPress up/down..."); ButtonPressedObserver observer = new ButtonPressedObserver(lcd); observer.addButtonListener(new ButtonListener() { @Override public void onButtonPressed(Button button) { try { switch (button) { case UP: currentTest = --currentTest < 0 ? 0 : currentTest; lcd.clear(); lcd.setText(String.format("#%d:%s\nPress Sel to run!", currentTest, TESTS[currentTest].getName())); break; case DOWN: currentTest = ++currentTest > (TESTS.length - 1) ? TESTS.length - 1 : currentTest; lcd.clear(); lcd.setText(String.format("#%d:%s\nPress Sel to run!", currentTest, TESTS[currentTest].getName())); break; case RIGHT: lcd.scrollDisplay(Direction.LEFT); break; case LEFT: lcd.scrollDisplay(Direction.RIGHT); break; case SELECT: runTest(currentTest); break; default: lcd.clear(); lcd.setText(String.format( "Button %s\nis not in use...", button.toString())); } } catch (IOException e) { handleException(e); } } private void runTest(int currentTest) { LCDDemo test = TESTS[currentTest]; System.out.println("Running test " + test.getName()); try { test.run(lcd); } catch (IOException e) { handleException(e); } } }); System.out.println("Press to quit!"); System.in.read(); lcd.stop(); } private static void handleException(IOException e) { System.out.println("Problem talking to LCD! Exiting!"); e.printStackTrace(); System.exit(2); } } \ No newline at end of file +/* * Copyright (c) 2014, 2024, Marcus Hirt, Miroslav Wengner * * Robo4J is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Robo4J is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Robo4J. If not, see . */ package com.robo4j.hw.rpi.i2c.adafruitlcd; import com.robo4j.hw.rpi.i2c.adafruitlcd.impl.AdafruitLcdImpl.Direction; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import java.io.IOException; /** * Here is a demonstration of things that can be done with the LCD shield. Run * this as a standalone java program. Do not forget to have sudo rights. * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Demo { private static final Logger LOGGER = LoggerFactory.getLogger(Demo.class); private static final LCDDemo[] TESTS = {new HelloWorldDemo(), new ScrollDemo(), new CursorDemo(), new DisplayDemo(), new ColorDemo(), new AutoScrollDemo(), new ExitDemo()}; private static int currentTest = -1; public static void main(String[] args) throws IOException { final AdafruitLcd lcd = LcdFactory.createLCD(); lcd.setText("LCD Test!\nPress up/down..."); ButtonPressedObserver observer = new ButtonPressedObserver(lcd); observer.addButtonListener(new ButtonListener() { @Override public void onButtonPressed(Button button) { try { switch (button) { case UP: currentTest = --currentTest < 0 ? 0 : currentTest; lcd.clear(); lcd.setText(String.format("#%d:%s\nPress Sel to run!", currentTest, TESTS[currentTest].getName())); break; case DOWN: currentTest = ++currentTest > (TESTS.length - 1) ? TESTS.length - 1 : currentTest; lcd.clear(); lcd.setText(String.format("#%d:%s\nPress Sel to run!", currentTest, TESTS[currentTest].getName())); break; case RIGHT: lcd.scrollDisplay(Direction.LEFT); break; case LEFT: lcd.scrollDisplay(Direction.RIGHT); break; case SELECT: runTest(currentTest); break; default: lcd.clear(); lcd.setText(String.format( "Button %s\nis not in use...", button.toString())); } } catch (IOException e) { handleException(e); } } private void runTest(int currentTest) { LCDDemo test = TESTS[currentTest]; LOGGER.debug("Running test {}", test.getName()); try { test.run(lcd); } catch (IOException e) { handleException(e); } } }); LOGGER.debug("Press to quit!"); System.in.read(); lcd.stop(); } private static void handleException(IOException e) { LOGGER.error("Problem talking to LCD! Exiting:{}", e.getMessage()); System.exit(2); } } \ No newline at end of file diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306DeviceTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306DeviceTest.java index e6cca4e8..585683eb 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306DeviceTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306DeviceTest.java @@ -16,66 +16,60 @@ */ package com.robo4j.hw.rpi.i2c.adafruitoled; -import java.awt.Color; -import java.awt.Graphics2D; -import java.awt.event.WindowAdapter; -import java.awt.event.WindowEvent; -import java.io.IOException; -import java.util.stream.Collectors; -import java.util.stream.Stream; - -import javax.swing.ImageIcon; -import javax.swing.JFrame; -import javax.swing.JLabel; - import com.robo4j.hw.rpi.i2c.adafruitoled.SSD1306Device.OLEDVariant; import com.robo4j.hw.rpi.utils.GpioPin; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.awt.*; +import java.io.IOException; /** * Example which prints Hello World and draws a little. It also shows the image * in a JFrame, so that it is easy to know what to expect. *

* Takes one argument, the number of lines of your specific device (32 or 64). - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class SSD1306DeviceTest { - private static final String DEFAULT_LINES = "32"; - private static final GpioPin RESET_PIN = GpioPin.GPIO_25; + private static final Logger LOGGER = LoggerFactory.getLogger(SSD1306DeviceTest.class); + private static final String DEFAULT_LINES = "32"; + private static final GpioPin RESET_PIN = GpioPin.GPIO_25; + + /** + * Start the example with either 32 or 64 as argument to select the number + * of lines. Will default to 32. + */ + public static void main(String[] args) throws IOException { + System.setProperty("java.awt.headless", "true"); + LOGGER.info("Headless:{}", System.getProperty("java.awt.headless")); + String lines = DEFAULT_LINES; + if (args.length > 0) { + lines = args[0]; + } - /** - * Start the example with either 32 or 64 as argument to select the number - * of lines. Will default to 32. - */ - public static void main(String[] args) throws IOException { - System.setProperty("java.awt.headless", "true"); - System.out.println("Headless:" + System.getProperty("java.awt.headless")); - String lines = DEFAULT_LINES; - if (args.length > 0) { - lines = args[0]; - } + OLEDVariant variant = lines.equals(DEFAULT_LINES) ? OLEDVariant.Type128x32 : OLEDVariant.Type128x64; + final SSD1306Device oled = new SSD1306Device(variant, RESET_PIN); - OLEDVariant variant = lines.equals(DEFAULT_LINES) ? OLEDVariant.Type128x32 : OLEDVariant.Type128x64; - final SSD1306Device oled = new SSD1306Device(variant, RESET_PIN); + LOGGER.info("Running OLED device example for {} with reset pin {}.", variant, RESET_PIN); + LOGGER.info("If the number of lines do not match your device,"); + LOGGER.info("please add the number of lines as the first argument!"); - System.out.println("Running OLED device example for " + variant + " with reset pin " + RESET_PIN + "."); - System.out.println("If the number of lines do not match your device,"); - System.out.println("please add the number of lines as the first argument!"); - - String text = args.length > 0 ? String.join(" ", args) : "Hello Maxi!"; - Graphics2D gc = oled.getGraphicsContext(); - gc.setColor(Color.white); - gc.setBackground(Color.black); - gc.clearRect(0, 0, 127, 31); - gc.drawLine(0, 0, 127, 31); - gc.drawString(text, 0, 30); - gc.setBackground(Color.white); - gc.fillOval(127 - 16, -16, 32, 32); - oled.pushImage(); - System.out.println("There is nothing"); + String text = args.length > 0 ? String.join(" ", args) : "Hello Maxi!"; + Graphics2D gc = oled.getGraphicsContext(); + gc.setColor(Color.white); + gc.setBackground(Color.black); + gc.clearRect(0, 0, 127, 31); + gc.drawLine(0, 0, 127, 31); + gc.drawString(text, 0, 30); + gc.setBackground(Color.white); + gc.fillOval(127 - 16, -16, 32, 32); + oled.pushImage(); + LOGGER.info("There is nothing"); - // TODO : create optional possibility to use JFrame as an output + // TODO : create optional possibility to use JFrame as an output // JFrame frame = new JFrame(); // frame.addWindowListener(new WindowAdapter() { // @@ -94,11 +88,11 @@ public static void main(String[] args) throws IOException { // frame.setSize(256, 256); // frame.getContentPane().add(new JLabel(new ImageIcon(oled.getImage()))); // frame.setVisible(true); - System.out.println("Press to quit!"); - System.in.read(); - oled.setEnabled(false); + LOGGER.info("Press to quit!"); + System.in.read(); + oled.setEnabled(false); // frame.setVisible(false); // frame.dispose(); - } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Test.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Test.java index cd95a983..1f415a90 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Test.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Test.java @@ -20,6 +20,8 @@ import com.robo4j.hw.rpi.i2c.bmp.BMP085Device; import com.robo4j.hw.rpi.i2c.bmp.BMP085Device.OperatingMode; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Repeatedly reads and displays the temperature (in C), pressure (in hPa) and @@ -29,10 +31,11 @@ * @author Miroslav Wengner (@miragemiko) */ public class BMP085Test { + private static final Logger LOGGER = LoggerFactory.getLogger(BMP085Test.class); public static void main(String[] args) throws IOException, InterruptedException { BMP085Device bmp = new BMP085Device(OperatingMode.STANDARD); while (true) { - System.out.printf("Temperature: %.1fC, Pressure: %dhPa, Altitude: %.1fm%n", + LOGGER.debug("Temperature: {}C, Pressure: {}hPa, Altitude: {}m", bmp.readTemperature(), bmp.readPressure() / 100, bmp.readAltitude()); diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gps/GPSTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gps/GPSTest.java index 206e0b3c..21e95e02 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gps/GPSTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gps/GPSTest.java @@ -20,6 +20,8 @@ import com.robo4j.hw.rpi.gps.GPSListener; import com.robo4j.hw.rpi.gps.PositionEvent; import com.robo4j.hw.rpi.gps.VelocityEvent; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -30,23 +32,25 @@ * @author Miroslav Wengner (@miragemiko) */ public class GPSTest { - // TODO remove duplicates - public static void main(String[] args) throws InterruptedException, IOException { - GPS sparkFunGPS = new TitanX1GPS(); - sparkFunGPS.addListener(new GPSListener() { - @Override - public void onPosition(PositionEvent event) { - System.out.println(event); - } + private static final Logger LOGGER = LoggerFactory.getLogger(GPSTest.class); - @Override - public void onVelocity(VelocityEvent event) { - System.out.println(event); - } - }); - sparkFunGPS.start(); - System.out.println("Press to quit!"); - System.in.read(); - sparkFunGPS.shutdown(); - } + // TODO remove duplicates + public static void main(String[] args) throws InterruptedException, IOException { + GPS sparkFunGPS = new TitanX1GPS(); + sparkFunGPS.addListener(new GPSListener() { + @Override + public void onPosition(PositionEvent event) { + LOGGER.debug("onPosition,event:{}", event); + } + + @Override + public void onVelocity(VelocityEvent event) { + LOGGER.debug("onVelocity,event:{}", event); + } + }); + sparkFunGPS.start(); + LOGGER.info("Press to quit!"); + System.in.read(); + sparkFunGPS.shutdown(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyroTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyroTest.java index 92518c4a..c612d2b7 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyroTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyroTest.java @@ -16,34 +16,37 @@ */ package com.robo4j.hw.rpi.i2c.gyro; -import java.io.IOException; - -import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device.Sensitivity; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Simple example which repeatedly reads the gyro. Good for checking that your * gyro is working. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class CalibratedGyroTest { + private static final Logger LOGGER = LoggerFactory.getLogger(CalibratedGyroTest.class); + + public static void main(String[] args) throws IOException, InterruptedException { + LOGGER.info("Initializing..."); + GyroL3GD20Device device = new GyroL3GD20Device(Sensitivity.DPS_245); + CalibratedGyro gyro = new CalibratedGyro(device); + + LOGGER.info("Keep the device still, and press enter to start calibration."); + System.in.read(); + LOGGER.info("Calibrating..."); + gyro.calibrate(); + LOGGER.info("Calibration done!"); - public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("Initializing..."); - GyroL3GD20Device device = new GyroL3GD20Device(Sensitivity.DPS_245); - CalibratedGyro gyro = new CalibratedGyro(device); - - System.out.println("Keep the device still, and press enter to start calibration."); - System.in.read(); - System.out.println("Calibrating..."); - gyro.calibrate(); - System.out.println("Calibration done!"); - - while (true) { - System.out.println(gyro.read()); - Thread.sleep(200); - } - } + // TODO : improve example + while (true) { + LOGGER.info("gyro, read:{}", gyro.read()); + Thread.sleep(200); + } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/GyroL3GD20Test.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/GyroL3GD20Test.java index a24ba2a0..4947886c 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/GyroL3GD20Test.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/gyro/GyroL3GD20Test.java @@ -16,27 +16,29 @@ */ package com.robo4j.hw.rpi.i2c.gyro; -import java.io.IOException; - -import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device.Sensitivity; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Simple example which repeatedly reads the gyro. Good for checking that your * gyro is working. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GyroL3GD20Test { + private static final Logger LOGGER = LoggerFactory.getLogger(GyroL3GD20Test.class); - public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("Initializing..."); - GyroL3GD20Device device = new GyroL3GD20Device(Sensitivity.DPS_245); + public static void main(String[] args) throws IOException, InterruptedException { + LOGGER.info("Initializing..."); + GyroL3GD20Device device = new GyroL3GD20Device(Sensitivity.DPS_245); - while (true) { - System.out.println(device.read()); - Thread.sleep(200); - } - } + while (true) { + LOGGER.info("device,read:{}", device.read()); + Thread.sleep(200); + } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/lidar/LidarLiteTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/lidar/LidarLiteTest.java index 28e38d52..9c1d0662 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/lidar/LidarLiteTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/lidar/LidarLiteTest.java @@ -16,26 +16,29 @@ */ package com.robo4j.hw.rpi.i2c.lidar; -import java.io.IOException; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import com.robo4j.hw.rpi.i2c.lidar.LidarLiteDevice; +import java.io.IOException; /** * This example will repeatedly acquire the range with the LidarLite. Good for * testing that it works. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class LidarLiteTest { - public static void main(String[] args) throws IOException, InterruptedException { - LidarLiteDevice ld = new LidarLiteDevice(); - while (true) { - ld.acquireRange(); - Thread.sleep(100); - System.out.printf("Distance: %.02fm%n", ld.readDistance()); - Thread.sleep(500); - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(LidarLiteTest.class); + + public static void main(String[] args) throws IOException, InterruptedException { + LidarLiteDevice ld = new LidarLiteDevice(); + while (true) { + ld.acquireRange(); + Thread.sleep(100); + LOGGER.info("Distance: {}m", ld.readDistance()); + Thread.sleep(500); + } + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/CalibratedMagnetometerExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/CalibratedMagnetometerExample.java index 344ff6f7..acd7f71a 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/CalibratedMagnetometerExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/CalibratedMagnetometerExample.java @@ -16,59 +16,62 @@ */ package com.robo4j.hw.rpi.i2c.magnetometer; -import java.io.IOException; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; - import com.robo4j.hw.rpi.i2c.magnetometer.MagnetometerLSM303Device.Mode; import com.robo4j.hw.rpi.i2c.magnetometer.MagnetometerLSM303Device.Rate; import com.robo4j.hw.rpi.utils.I2cBus; import com.robo4j.math.geometry.Matrix3f; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; /** * Magnetometer example for trying out a calibrated magnetometer. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class CalibratedMagnetometerExample { - private final static ScheduledExecutorService EXECUTOR = Executors.newScheduledThreadPool(1); + private static final Logger LOGGER = LoggerFactory.getLogger(CalibratedMagnetometerExample.class); + private final static ScheduledExecutorService EXECUTOR = Executors.newScheduledThreadPool(1); - private static class MagnetometerPoller implements Runnable { - private final MagnetometerLSM303Device magDevice; + private static class MagnetometerPoller implements Runnable { + private final MagnetometerLSM303Device magDevice; - public MagnetometerPoller(MagnetometerLSM303Device magDevice) { - this.magDevice = magDevice; - } + public MagnetometerPoller(MagnetometerLSM303Device magDevice) { + this.magDevice = magDevice; + } - @Override - public void run() { - try { - Tuple3f magResult = magDevice.read(); - System.out.printf("Heading: %3.2f%n", MagnetometerLSM303Device.getCompassHeading(magResult)); - } catch (IOException e) { - e.printStackTrace(); - } - } - } + @Override + public void run() { + try { + Tuple3f magResult = magDevice.read(); + LOGGER.info("Heading:{}", MagnetometerLSM303Device.getCompassHeading(magResult)); + } catch (IOException e) { + LOGGER.error(e.getMessage(), e); + } + } + } - public static void main(String[] args) throws IOException, InterruptedException { - // NOTE(Marcus/Jul 23, 2017): Change the actual bias vector and - // transform matrix to match your calibration values. - // See the MagViz tool for more information. - Tuple3f bias = new Tuple3f(-44.689f, -2.0665f, -15.240f); - Matrix3f transform = new Matrix3f(1.887f, 5.987f, -5.709f, 5.987f, 1.528f, -2.960f, -5.709f, -2.960f, 9.761f); - MagnetometerLSM303Device magnetometer = new MagnetometerLSM303Device(I2cBus.BUS_1, 0x1e, Mode.CONTINUOUS_CONVERSION, Rate.RATE_7_5, - false, bias, transform); - System.out.println( - "Starting to read and print the heading. Make sure the magnetometer is flat in the XY-plane (this example does not do tilt compensated heading)."); - System.out.println("Press to quit..."); - EXECUTOR.scheduleAtFixedRate(new MagnetometerPoller(magnetometer), 100, 500, TimeUnit.MILLISECONDS); - System.in.read(); - EXECUTOR.shutdown(); - EXECUTOR.awaitTermination(1, TimeUnit.SECONDS); - System.out.println("Goodbye!"); - } + public static void main(String[] args) throws IOException, InterruptedException { + // NOTE(Marcus/Jul 23, 2017): Change the actual bias vector and + // transform matrix to match your calibration values. + // See the MagViz tool for more information. + Tuple3f bias = new Tuple3f(-44.689f, -2.0665f, -15.240f); + Matrix3f transform = new Matrix3f(1.887f, 5.987f, -5.709f, 5.987f, 1.528f, -2.960f, -5.709f, -2.960f, 9.761f); + MagnetometerLSM303Device magnetometer = new MagnetometerLSM303Device(I2cBus.BUS_1, 0x1e, Mode.CONTINUOUS_CONVERSION, Rate.RATE_7_5, + false, bias, transform); + LOGGER.info( + "Starting to read and print the heading. Make sure the magnetometer is flat in the XY-plane (this example does not do tilt compensated heading)."); + LOGGER.info("Press to quit..."); + EXECUTOR.scheduleAtFixedRate(new MagnetometerPoller(magnetometer), 100, 500, TimeUnit.MILLISECONDS); + System.in.read(); + EXECUTOR.shutdown(); + EXECUTOR.awaitTermination(1, TimeUnit.SECONDS); + LOGGER.info("Goodbye!"); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Example.java index 22ab17f0..95732def 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Example.java @@ -16,122 +16,126 @@ */ package com.robo4j.hw.rpi.i2c.magnetometer; +import com.robo4j.math.geometry.Tuple3f; +import com.robo4j.math.geometry.Tuple3i; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.util.concurrent.Executors; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; -import com.robo4j.math.geometry.Tuple3f; -import com.robo4j.math.geometry.Tuple3i; - /** * Example program that can be used to produce csv data that can be used for * calibration. - * + * *

* Example: MagnetometerLSM303Test 100 1 csv *

- * + * *

See the MagViz utility.

- * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MagnetometerLSM303Example { - private final static ScheduledExecutorService EXECUTOR_SERVICE = Executors.newSingleThreadScheduledExecutor(); + private static final Logger LOGGER = LoggerFactory.getLogger(MagnetometerLSM303Example.class); + private final static ScheduledExecutorService EXECUTOR_SERVICE = Executors.newSingleThreadScheduledExecutor(); - enum PrintStyle { - PRETTY, RAW, CSV - } + enum PrintStyle { + PRETTY, RAW, CSV + } - private static class DataGatherer implements Runnable { - private final MagnetometerLSM303Device device; - private final int modulo; - private int count; - private final PrintStyle printStyle; + private static class DataGatherer implements Runnable { + private final MagnetometerLSM303Device device; + private final int modulo; + private int count; + private final PrintStyle printStyle; - public DataGatherer(int modulo, PrintStyle printStyle) throws IOException { - this.modulo = modulo; - this.printStyle = printStyle; - device = new MagnetometerLSM303Device(); - } + public DataGatherer(int modulo, PrintStyle printStyle) throws IOException { + this.modulo = modulo; + this.printStyle = printStyle; + device = new MagnetometerLSM303Device(); + } - // TODO : improve by simple logging - @Override - public void run() { - if (count % modulo == 0) { - switch (printStyle) { - case RAW: - Tuple3i raw = readRaw(); - System.out.printf("Raw Value %d = %s%n", count, raw.toString()); - break; - case CSV: - Tuple3f val = read(); - System.out.printf("%f;%f;%f%n", val.x, val.y, val.z); - break; - default: - val = read(); - System.out.println(String.format("Value %d = %s\\tHeading:%f", count, val.toString(), - MagnetometerLSM303Device.getCompassHeading(val))); - } - } - count++; - } + // TODO : improve by simple logging + @Override + public void run() { + if (count % modulo == 0) { + switch (printStyle) { + case RAW: + Tuple3i raw = readRaw(); + LOGGER.debug("Raw Value {}} = {}", count, raw.toString()); + break; + case CSV: + Tuple3f val = read(); + LOGGER.debug("{};{};{}", val.x, val.y, val.z); + break; + default: + val = read(); + var message = String.format("Value %d = %s\\tHeading:%f", count, val.toString(), + MagnetometerLSM303Device.getCompassHeading(val)); + LOGGER.debug("{}", message); + } + } + count++; + } - private Tuple3i readRaw() { - try { - return device.readRaw(); - } catch (IOException e) { - e.printStackTrace(); - System.exit(3); - } - return null; - } + private Tuple3i readRaw() { + try { + return device.readRaw(); + } catch (IOException e) { + LOGGER.error("Error reading raw data", e); + System.exit(3); + } + return null; + } - private Tuple3f read() { - try { - return device.read(); - } catch (IOException e) { - e.printStackTrace(); - System.exit(4); - } - return null; - } + private Tuple3f read() { + try { + return device.read(); + } catch (IOException e) { + LOGGER.error("Error reading data", e); + System.exit(4); + } + return null; + } - public int getCount() { - return count; - } - } + public int getCount() { + return count; + } + } - // FIXME(Marcus/Dec 5, 2016): Verify that this one works. - public static void main(String[] args) throws IOException, InterruptedException { - if (args.length < 2) { - System.out.println( - "Usage: MagnetometerLSM303Test [] "); - System.exit(1); - } - int period = Integer.parseInt(args[0]); - int modulo = Integer.parseInt(args[1]); - PrintStyle printStyle = PrintStyle.PRETTY; - if (args.length >= 3) { - printStyle = PrintStyle.valueOf(args[2].toUpperCase()); - } - DataGatherer dg = new DataGatherer(modulo, printStyle); - printMessage(printStyle, "Starting to collect data..."); - printMessage(printStyle, "Press to stop!"); - EXECUTOR_SERVICE.scheduleAtFixedRate(dg, 0, period, TimeUnit.MILLISECONDS); - System.in.read(); - EXECUTOR_SERVICE.shutdown(); - EXECUTOR_SERVICE.awaitTermination(1, TimeUnit.SECONDS); - printMessage(printStyle, "Collected " + dg.getCount() + " values!"); - } + // FIXME(Marcus/Dec 5, 2016): Verify that this one works. + public static void main(String[] args) throws IOException, InterruptedException { + if (args.length < 2) { + LOGGER.info( + "Usage: MagnetometerLSM303Test [] "); + System.exit(1); + } + int period = Integer.parseInt(args[0]); + int modulo = Integer.parseInt(args[1]); + PrintStyle printStyle = PrintStyle.PRETTY; + if (args.length >= 3) { + printStyle = PrintStyle.valueOf(args[2].toUpperCase()); + } + DataGatherer dg = new DataGatherer(modulo, printStyle); + printMessage(printStyle, "Starting to collect data..."); + printMessage(printStyle, "Press to stop!"); + EXECUTOR_SERVICE.scheduleAtFixedRate(dg, 0, period, TimeUnit.MILLISECONDS); + System.in.read(); + EXECUTOR_SERVICE.shutdown(); + EXECUTOR_SERVICE.awaitTermination(1, TimeUnit.SECONDS); + printMessage(printStyle, "Collected " + dg.getCount() + " values!"); + } - private static void printMessage(PrintStyle printStyle, String message) { - System.out.println(getPrefix(printStyle) + message); + private static void printMessage(PrintStyle printStyle, String message) { + LOGGER.info("{}{}", getPrefix(printStyle), message); - } + } - private static String getPrefix(PrintStyle printStyle) { - return printStyle == PrintStyle.CSV ? "# " : ""; - } + private static String getPrefix(PrintStyle printStyle) { + return printStyle == PrintStyle.CSV ? "# " : ""; + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/MC33926Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/MC33926Example.java index 2284d8d4..a35e7dc5 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/MC33926Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/MC33926Example.java @@ -17,6 +17,8 @@ package com.robo4j.hw.rpi.i2c.pwm; import com.robo4j.hw.rpi.utils.GpioPin; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -29,13 +31,13 @@ * @author Miroslav Wengner (@miragemiko) */ public class MC33926Example { - + private static final Logger LOGGER = LoggerFactory.getLogger(MC33926Example.class); public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("Creating device..."); + LOGGER.info("Creating device..."); if (args.length != 3) { - System.out.println("Usage: MC33926Example "); + LOGGER.info("Usage: MC33926Example "); System.out.flush(); System.exit(2); } @@ -50,12 +52,12 @@ private static void runEngine(float speed, int duration) throws IOException, Int HBridgeMC33926Device engine = new HBridgeMC33926Device("Engine", pwm.getChannel(4), GpioPin.GPIO_02, GpioPin.GPIO_03, true); - System.out.printf("Running for %d ms at speed %f...%n", duration, speed); + LOGGER.info("Running for {}ms at speed:{}...%n", duration, speed); engine.setSpeed(speed); Thread.sleep(duration); engine.setSpeed(0); - System.out.println("Done!"); + LOGGER.info("Done!"); } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Example.java index ab1bac22..7683ed4f 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Example.java @@ -22,6 +22,8 @@ import com.robo4j.hw.rpi.Servo; import com.robo4j.hw.rpi.i2c.pwm.PCA9685Servo; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device.PWMChannel; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * This example assumes two servos connected to channel 0 and 1, and two H bridges controlling DC engines on channel 2 and 3. @@ -33,6 +35,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class PCA9685Example { + private static final Logger LOGGER = LoggerFactory.getLogger(PCA9685Example.class); // The internetz says 50Hz is the standard PWM frequency for operating RC servos. private static final int SERVO_FREQUENCY = 50; private static final int MOTOR_MIN = 0; @@ -41,23 +44,23 @@ public class PCA9685Example { public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("Creating device..."); + LOGGER.info("Creating device..."); PWMPCA9685Device device = new PWMPCA9685Device(); device.setPWMFrequency(SERVO_FREQUENCY); Servo servo0 = new PCA9685Servo(device.getChannel(0)); Servo servo1 = new PCA9685Servo(device.getChannel(1)); PWMChannel motor0 = device.getChannel(2); PWMChannel motor1 = device.getChannel(3); - - System.out.println("Setting start conditions..."); + + LOGGER.info("Setting start conditions..."); servo0.setInput(0); servo1.setInput(0); motor0.setPWM(0, MOTOR_MIN); motor1.setPWM(0, MOTOR_MIN); - System.out.println("Press to run loop!"); + LOGGER.info("Press to run loop!"); System.in.read(); - System.out.println("Running perpetual loop..."); + LOGGER.info("Running perpetual loop..."); while (true) { servo0.setInput(-1); servo1.setInput(-1); diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/ServoTester.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/ServoTester.java index 4c555cb4..6c0f2a7d 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/ServoTester.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/i2c/pwm/ServoTester.java @@ -16,78 +16,81 @@ */ package com.robo4j.hw.rpi.i2c.pwm; +import com.robo4j.hw.rpi.Servo; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.util.Scanner; -import com.robo4j.hw.rpi.Servo; - /** * This is a simple example allowing you to try out the servos connected to a * PCA9685. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class ServoTester { - // The internetz says 50Hz is the standard PWM frequency for operating RC - // servos. - private static final int SERVO_FREQUENCY = 50; - private static final Servo[] SERVOS = new Servo[16]; + private static final Logger LOGGER = LoggerFactory.getLogger(ServoTester.class); + // The internetz says 50Hz is the standard PWM frequency for operating RC + // servos. + private static final int SERVO_FREQUENCY = 50; + private static final Servo[] SERVOS = new Servo[16]; - public static void main(String[] args) throws IOException, InterruptedException { - System.out.print("Creating device..."); - PWMPCA9685Device device = new PWMPCA9685Device(); - device.setPWMFrequency(SERVO_FREQUENCY); - System.out.println("done!"); - System.out.println( - "Type the id of the channel of the servo to control and how much to move the servo,\nbetween -1 and 1. For example:\nknown servos=0>15 -1.0\nType q and enter to quit!\n"); - System.out.flush(); - Scanner scanner = new Scanner(System.in); - String lastCommand; - printPrompt(); - while (!"q".equals(lastCommand = scanner.nextLine())) { - lastCommand = lastCommand.trim(); - String[] split = lastCommand.split(" "); - if (split.length != 2) { - System.out.println("Could not parse " + lastCommand + ". Please try again!"); - continue; - } - int channel = Integer.parseInt(split[0]); - float position = Float.parseFloat(split[1]); + public static void main(String[] args) throws IOException, InterruptedException { + LOGGER.info("Creating device..."); + PWMPCA9685Device device = new PWMPCA9685Device(); + device.setPWMFrequency(SERVO_FREQUENCY); + LOGGER.info("done!"); + LOGGER.info( + "Type the id of the channel of the servo to control and how much to move the servo,\nbetween -1 and 1. For example:\nknown servos=0>15 -1.0\nType q and enter to quit!\n"); + System.out.flush(); + Scanner scanner = new Scanner(System.in); + String lastCommand; + printPrompt(); + while (!"q".equals(lastCommand = scanner.nextLine())) { + lastCommand = lastCommand.trim(); + String[] split = lastCommand.split(" "); + if (split.length != 2) { + LOGGER.debug("Could not parse {}. Please try again!", lastCommand); + continue; + } + int channel = Integer.parseInt(split[0]); + float position = Float.parseFloat(split[1]); - if (channel < 0 || channel > 15) { - System.out.println("Channel number " + channel + " is not allowed! Try again..."); - continue; - } - Servo servo = SERVOS[channel]; + if (channel < 0 || channel > 15) { + LOGGER.debug("Channel number {} is not allowed! Try again...", channel); + continue; + } + Servo servo = SERVOS[channel]; - if (servo == null) { - servo = new PCA9685Servo(device.getChannel(channel)); - SERVOS[channel] = servo; - } + if (servo == null) { + servo = new PCA9685Servo(device.getChannel(channel)); + SERVOS[channel] = servo; + } - if (position < -1 || position > 1) { - System.out.println("Input " + position + " is not allowed! Try again..."); - continue; - } - servo.setInput(position); - printPrompt(); - } - scanner.close(); - System.out.println("Bye!"); - } + if (position < -1 || position > 1) { + LOGGER.debug("Input {} is not allowed! Try again...", position); + continue; + } + servo.setInput(position); + printPrompt(); + } + scanner.close(); + LOGGER.info("Bye!"); + } - private static void printPrompt() { - System.out.printf("known servos=%d>", getNumberOfKnownServos()); - } + private static void printPrompt() { + LOGGER.info("known servos={}", getNumberOfKnownServos()); + } - private static int getNumberOfKnownServos() { - int count = 0; + private static int getNumberOfKnownServos() { + int count = 0; for (Servo servo : SERVOS) { if (servo != null) { count++; } } - return count; - } + return count; + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno055Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno055Example.java index 6af1d39e..578ecea5 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno055Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno055Example.java @@ -18,10 +18,12 @@ import com.robo4j.hw.rpi.imu.bno.Bno055CalibrationStatus; import com.robo4j.hw.rpi.imu.bno.Bno055Device; +import com.robo4j.hw.rpi.imu.bno.Bno055Device.OperatingMode; import com.robo4j.hw.rpi.imu.bno.Bno055Factory; import com.robo4j.hw.rpi.imu.bno.Bno055SelfTestResult; -import com.robo4j.hw.rpi.imu.bno.Bno055Device.OperatingMode; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.concurrent.Executors; @@ -30,88 +32,82 @@ /** * An example for the BNO device. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Bno055Example { - private final static ScheduledExecutorService EXECUTOR = Executors.newScheduledThreadPool(1); - - private final static class BNOPrinter implements Runnable { - - private final Bno055Device device; - - private BNOPrinter(Bno055Device device) { - this.device = device; - } - - @Override - public void run() { - try { - Tuple3f orientation = device.read(); - float temperature = device.getTemperature(); - - System.out.printf("heading: %f, roll: %f, pitch: %f - temp:%f%n", orientation.x, - orientation.y, orientation.z, temperature); - } catch (Throwable e) { - // TODO : improve exceptions - e.printStackTrace(); - } - } - - } - - /** - * Runs an example for the BNO running in serial. Use the appropriate factory - * method to instead use I2C. - * - * @param args - * arguments - * @throws IOException - * exception - * @throws InterruptedException - * exception - */ - public static void main(String[] args) throws IOException, InterruptedException { - System.out.println("Starting the BNO055 Example."); - Bno055Device bno = Bno055Factory.createDefaultSerialDevice(); - Thread.sleep(20); - - System.out.println("Resetting device..."); - bno.reset(); - Thread.sleep(20); - - System.out.println("Running Self Test..."); - Bno055SelfTestResult testResult = bno.performSelfTest(); - System.out.println("Result of self test: "); - System.out.println(testResult); - Thread.sleep(20); - - System.out.println("Operating mode: " + bno.getOperatingMode()); - if (bno.getOperatingMode() != OperatingMode.NDOF) { - System.out.println("Switching mode to NDOF"); - bno.setOperatingMode(OperatingMode.NDOF); - System.out.println("Operating mode: " + bno.getOperatingMode()); - } - - System.out.println("Starting calibration sequence..."); - Bno055CalibrationStatus calibrationStatus = null; - while (!(calibrationStatus = bno.getCalibrationStatus()).isFullyCalibrated()) { - System.out.println(String.format( - "Calibration status: system:%s, gyro:%s, accelerometer:%s, magnetometer:%s", - calibrationStatus.getSystemCalibrationStatus(), calibrationStatus.getGyroCalibrationStatus(), - calibrationStatus.getAccelerometerCalibrationStatus(), - calibrationStatus.getAccelerometerCalibrationStatus())); - Thread.sleep(500); - } - System.out.println("System fully calibrated. Now printing data. Press to quit!"); - - EXECUTOR.scheduleAtFixedRate(new BNOPrinter(bno), 40, 500, TimeUnit.MILLISECONDS); - System.in.read(); - EXECUTOR.shutdown(); - EXECUTOR.awaitTermination(1000, TimeUnit.MILLISECONDS); - System.out.println("Bye, bye!"); - bno.shutdown(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(Bno055Example.class); + private final static ScheduledExecutorService EXECUTOR = Executors.newScheduledThreadPool(1); + + private final static class BNOPrinter implements Runnable { + + private final Bno055Device device; + + private BNOPrinter(Bno055Device device) { + this.device = device; + } + + @Override + public void run() { + try { + Tuple3f orientation = device.read(); + float temperature = device.getTemperature(); + + LOGGER.info("heading: {}, roll: {}, pitch: {} - temp:{}", orientation.x, + orientation.y, orientation.z, temperature); + } catch (Throwable e) { + LOGGER.error("error", e); + } + } + + } + + /** + * Runs an example for the BNO running in serial. Use the appropriate factory + * method to instead use I2C. + * + * @param args arguments + * @throws IOException exception + * @throws InterruptedException exception + */ + public static void main(String[] args) throws IOException, InterruptedException { + // TODO: review sleeps + LOGGER.info("Starting the BNO055 Example."); + Bno055Device bno = Bno055Factory.createDefaultSerialDevice(); + Thread.sleep(20); + + LOGGER.info("Resetting device..."); + bno.reset(); + Thread.sleep(20); + + LOGGER.info("Running Self Test..."); + Bno055SelfTestResult testResult = bno.performSelfTest(); + LOGGER.info("Result of self test: "); + LOGGER.info("result:{}", testResult); + Thread.sleep(20); + + LOGGER.info("Operating mode: {}", bno.getOperatingMode()); + if (bno.getOperatingMode() != OperatingMode.NDOF) { + LOGGER.info("Switching mode to NDOF"); + bno.setOperatingMode(OperatingMode.NDOF); + LOGGER.info("Operating mode: {}", bno.getOperatingMode()); + } + + LOGGER.info("Starting calibration sequence..."); + Bno055CalibrationStatus calibrationStatus = null; + while (!(calibrationStatus = bno.getCalibrationStatus()).isFullyCalibrated()) { + LOGGER.info("Calibration status: system:{}, gyro:{}, accelerometer:{}, magnetometer:{}", calibrationStatus.getSystemCalibrationStatus(), calibrationStatus.getGyroCalibrationStatus(), calibrationStatus.getAccelerometerCalibrationStatus(), calibrationStatus.getAccelerometerCalibrationStatus()); + Thread.sleep(500); + } + LOGGER.info("System fully calibrated. Now printing data. Press to quit!"); + + EXECUTOR.scheduleAtFixedRate(new BNOPrinter(bno), 40, 500, TimeUnit.MILLISECONDS); + System.in.read(); + EXECUTOR.shutdown(); + EXECUTOR.awaitTermination(1000, TimeUnit.MILLISECONDS); + LOGGER.info("Bye, bye!"); + bno.shutdown(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080AccelerometerExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080AccelerometerExample.java index c44ff281..7ebf506f 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080AccelerometerExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080AccelerometerExample.java @@ -22,24 +22,27 @@ import com.robo4j.hw.rpi.imu.bno.DataEvent3f; import com.robo4j.hw.rpi.imu.bno.DataListener; import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Prints out accelerometer information to the console. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Bno080AccelerometerExample { + private static final Logger LOGGER = LoggerFactory.getLogger(Bno080AccelerometerExample.class); - public static void main(String[] args) throws Exception { - DataListener listener = (DataEvent3f event) -> System.out.println("ShtpPacketResponse: " + event); - System.out.println("BNO080 SPI Accelerometer Example"); - // Change here to use other modes of communication - Bno080Device device = Bno080Factory.createDefaultSPIDevice(); - device.addListener(listener); - device.start(SensorReportId.ACCELEROMETER, 100); - System.out.println("Press to quit!"); - System.in.read(); - device.shutdown(); - } + public static void main(String[] args) throws Exception { + DataListener listener = (DataEvent3f event) -> LOGGER.info("ShtpPacketResponse: {}", event); + LOGGER.info("BNO080 SPI Accelerometer Example"); + // Change here to use other modes of communication + Bno080Device device = Bno080Factory.createDefaultSPIDevice(); + device.addListener(listener); + device.start(SensorReportId.ACCELEROMETER, 100); + LOGGER.info("Press to quit!"); + System.in.read(); + device.shutdown(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080Example.java index 64a9fefc..63a5f2b2 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/imu/Bno080Example.java @@ -22,24 +22,27 @@ import com.robo4j.hw.rpi.imu.bno.DataEvent3f; import com.robo4j.hw.rpi.imu.bno.DataListener; import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Prints out rotation vector information to the console. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Bno080Example { + private static final Logger LOGGER = LoggerFactory.getLogger(Bno080Example.class); - public static void main(String[] args) throws Exception { - DataListener listener = (DataEvent3f event) -> System.out.println("ShtpPacketResponse: " + event); - System.out.println("BNO080 SPI Rotation Vector Example"); - // Change here to use other modes of communication - Bno080Device device = Bno080Factory.createDefaultSPIDevice(); - device.addListener(listener); - device.start(SensorReportId.ROTATION_VECTOR, 1000); - System.out.println("Press to quit!"); - System.in.read(); - device.shutdown(); - } + public static void main(String[] args) throws Exception { + DataListener listener = (DataEvent3f event) -> LOGGER.info("ShtpPacketResponse: {}", event); + LOGGER.info("BNO080 SPI Rotation Vector Example"); + // Change here to use other modes of communication + Bno080Device device = Bno080Factory.createDefaultSPIDevice(); + device.addListener(listener); + device.start(SensorReportId.ROTATION_VECTOR, 1000); + LOGGER.info("Press to quit!"); + System.in.read(); + device.shutdown(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/lcd/Lcd20x4Example.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/lcd/Lcd20x4Example.java index c3962b08..55809a1f 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/lcd/Lcd20x4Example.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/lcd/Lcd20x4Example.java @@ -16,66 +16,69 @@ */ package com.robo4j.hw.rpi.lcd; -import java.io.IOException; - import com.robo4j.hw.rpi.lcd.Lcd20x4.Alignment; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Demo for the 20x4 LCD. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Lcd20x4Example { + private static final Logger LOGGER = LoggerFactory.getLogger(Lcd20x4Example.class); - private static class Demo implements Runnable { - private final Lcd20x4 lcd; - private volatile boolean shouldRun = true; + private static class Demo implements Runnable { + private final Lcd20x4 lcd; + private volatile boolean shouldRun = true; - Demo(Lcd20x4 lcd) { - this.lcd = lcd; - } + Demo(Lcd20x4 lcd) { + this.lcd = lcd; + } - @Override - public void run() { - while (shouldRun) { - lcd.sendMessage(1, "--------------------", Alignment.CENTER); - lcd.sendMessage(2, "Rasbperry Pi", Alignment.CENTER); - lcd.sendMessage(3, "Robo4J", Alignment.CENTER); - lcd.sendMessage(4, "--------------------", Alignment.CENTER); - sleep(3000); - lcd.sendMessage(1, "--------------------", Alignment.CENTER); - lcd.sendMessage(2, "This is a test", Alignment.CENTER); - lcd.sendMessage(3, "20x4 LCD Module", Alignment.CENTER); - lcd.sendMessage(4, "--------------------", Alignment.CENTER); - sleep(3000); - } - lcd.clearDisplay(); - lcd.sendMessage(2, "Goodbye!", Alignment.CENTER); - } + @Override + public void run() { + while (shouldRun) { + lcd.sendMessage(1, "--------------------", Alignment.CENTER); + lcd.sendMessage(2, "Rasbperry Pi", Alignment.CENTER); + lcd.sendMessage(3, "Robo4J", Alignment.CENTER); + lcd.sendMessage(4, "--------------------", Alignment.CENTER); + sleep(3000); + lcd.sendMessage(1, "--------------------", Alignment.CENTER); + lcd.sendMessage(2, "This is a test", Alignment.CENTER); + lcd.sendMessage(3, "20x4 LCD Module", Alignment.CENTER); + lcd.sendMessage(4, "--------------------", Alignment.CENTER); + sleep(3000); + } + lcd.clearDisplay(); + lcd.sendMessage(2, "Goodbye!", Alignment.CENTER); + } - private void sleep(int seconds) { - if (shouldRun) { - try { - Thread.sleep(seconds); - } catch (InterruptedException e) { - // Don't care - } - } - } + private void sleep(int seconds) { + if (shouldRun) { + try { + Thread.sleep(seconds); + } catch (InterruptedException e) { + // Don't care + } + } + } - public void quit() { - shouldRun = false; - } - } + public void quit() { + shouldRun = false; + } + } - public static void main(String[] args) throws IOException { - Lcd20x4 lcd = new Lcd20x4(); - Demo demo = new Demo(lcd); - Thread t = new Thread(demo, "LCD Demo Thread"); - t.start(); - System.out.println("Running LCD demo. Press to quit!"); - System.in.read(); - demo.quit(); - } + public static void main(String[] args) throws IOException { + Lcd20x4 lcd = new Lcd20x4(); + Demo demo = new Demo(lcd); + Thread t = new Thread(demo, "LCD Demo Thread"); + t.start(); + LOGGER.debug("Running LCD demo. Press to quit!"); + System.in.read(); + demo.quit(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pad/LF710PadExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pad/LF710PadExample.java index c9c96c27..6b260961 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pad/LF710PadExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pad/LF710PadExample.java @@ -16,6 +16,9 @@ */ package com.robo4j.hw.rpi.pad; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.io.InputStream; import java.nio.file.Files; @@ -25,14 +28,14 @@ /** * Java port of Logitech F710 Gamepad * optimised of raspberryPi - * + *

* note: set the F710 pad to DirectInput by front button * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class LF710PadExample implements Runnable { - + private static final Logger LOGGER = LoggerFactory.getLogger(LF710PadExample.class); private static final String REGISTERED_INPUT = "/dev/input/js0"; private static final int ANDING_LEFT = 0x00ff; private static final int ANDING_LONG_LEFT = 0x00000000000000ff; @@ -48,7 +51,7 @@ public class LF710PadExample implements Runnable { private final Path inputPath = Paths.get(REGISTERED_INPUT); public static void main(String[] args) { - System.out.println("start joystick logitech F710"); + LOGGER.info("start joystick logitech F710"); LF710PadExample pad = new LF710PadExample(); new Thread(pad).start(); } @@ -63,24 +66,22 @@ public void run() { try { final InputStream inputStream = Files.newInputStream(inputPath); int bytes; - while((bytes = inputStream.read(buffer)) > INDEX_START){ - if(bytes == BUFFER_SIZE){ + while ((bytes = inputStream.read(buffer)) > INDEX_START) { + if (bytes == BUFFER_SIZE) { final long time = ((((((buffer[INDEX_TIME_3] & ANDING_LONG_LEFT) << BUFFER_SIZE) | (buffer[INDEX_TIME_2] & ANDING_LEFT)) << BUFFER_SIZE) | (buffer[INDEX_TIME_1] & ANDING_LEFT)) << BUFFER_SIZE) | (buffer[INDEX_START] & ANDING_LEFT); final short amount = (short) (((buffer[INDEX_AMOUNT_5] & ANDING_LEFT) << BUFFER_SIZE) | (buffer[INDEX_AMOUNT_4] & ANDING_LEFT)); final short part = buffer[INDEX_PART]; final short element = buffer[INDEX_ELEMENT]; - if(part > 0){ + if (part > 0) { final LF710Part lf710Part = LF710Part.getByMask(part); - switch (lf710Part){ + switch (lf710Part) { case BUTTON: - System.out.println("BUTTON: " + LF710Button.getByMask(element) + " state: " + - getInputState(amount) + " time: " + time); + LOGGER.info("BUTTON: {} state: {} time: {}", LF710Button.getByMask(element), getInputState(amount), time); break; case JOYSTICK: - System.out.println("JOYSTICK: " + LF710JoystickButton.getByMask(element) + " state: " + - getInputState(amount) + " time: " + time); + LOGGER.info("JOYSTICK: {} state: {} time: {}", LF710JoystickButton.getByMask(element), getInputState(amount), time); break; default: throw new RuntimeException("uknonw pad part:" + lf710Part); @@ -94,7 +95,7 @@ public void run() { } //Private Methods - private LF710State getInputState(short amount){ + private LF710State getInputState(short amount) { return (amount == 0) ? LF710State.RELEASED : LF710State.PRESSED; } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PCA9685TruckPlatformExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PCA9685TruckPlatformExample.java index bebd685b..1640214e 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PCA9685TruckPlatformExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PCA9685TruckPlatformExample.java @@ -19,6 +19,8 @@ import com.robo4j.hw.rpi.Servo; import com.robo4j.hw.rpi.i2c.pwm.PCA9685Servo; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -29,51 +31,51 @@ * @author Miroslav Wengner (@miragemiko) */ public class PCA9685TruckPlatformExample { + private static final Logger LOGGER = LoggerFactory.getLogger(PCA9685TruckPlatformExample.class); - private static final int SERVO_THROTTLE = 0; - private static final int SERVO_STEERING = 5; - private static final int SERVO_LEG = 6; - private static final int SERVO_SHIFT = 7; - private static final int SERVO_FREQUENCY = 250; + private static final int SERVO_THROTTLE = 0; + private static final int SERVO_STEERING = 5; + private static final int SERVO_LEG = 6; + private static final int SERVO_SHIFT = 7; + private static final int SERVO_FREQUENCY = 250; - public static void main(String[] args) throws IOException, InterruptedException { - if (args.length != 5) { - System.out.printf("Usage: %s %n", - PCA9685TruckPlatformExample.class.getSimpleName()); - System.out.flush(); - System.exit(2); - } - float throttle = Float.parseFloat(args[0]); - float steering = Float.parseFloat(args[1]); - float leg = Float.parseFloat(args[2]); - float shift = Float.parseFloat(args[3]); - int duration = Integer.parseInt(args[4]); + public static void main(String[] args) throws IOException, InterruptedException { + if (args.length != 5) { + LOGGER.info("Usage: "); + System.out.flush(); + System.exit(2); + } + float throttle = Float.parseFloat(args[0]); + float steering = Float.parseFloat(args[1]); + float leg = Float.parseFloat(args[2]); + float shift = Float.parseFloat(args[3]); + int duration = Integer.parseInt(args[4]); - testMotor(throttle, steering, leg, shift, duration); - System.out.println("All done! Bye!"); - } + testMotor(throttle, steering, leg, shift, duration); + LOGGER.info("All done! Bye!"); + } - public static void testMotor(float throttle, float steering, float leg, float shift, int duration) - throws IOException, InterruptedException { - System.out.println(String.format("Running for %d ms with throttle %f, steering %f, leg %f, shift %f", duration, - throttle, steering, leg, shift)); - PWMPCA9685Device device = new PWMPCA9685Device(); - device.setPWMFrequency(SERVO_FREQUENCY); - Servo throttleEngine = new PCA9685Servo(device.getChannel(SERVO_THROTTLE)); - Servo steeringEngine = new PCA9685Servo(device.getChannel(SERVO_STEERING)); - Servo legEngine = new PCA9685Servo(device.getChannel(SERVO_LEG)); - Servo shiftEngine = new PCA9685Servo(device.getChannel(SERVO_SHIFT)); + public static void testMotor(float throttle, float steering, float leg, float shift, int duration) + throws IOException, InterruptedException { + LOGGER.debug("Running for {} ms with throttle {}, steering {}, leg {}, shift {}", duration, + throttle, steering, leg, shift); + PWMPCA9685Device device = new PWMPCA9685Device(); + device.setPWMFrequency(SERVO_FREQUENCY); + Servo throttleEngine = new PCA9685Servo(device.getChannel(SERVO_THROTTLE)); + Servo steeringEngine = new PCA9685Servo(device.getChannel(SERVO_STEERING)); + Servo legEngine = new PCA9685Servo(device.getChannel(SERVO_LEG)); + Servo shiftEngine = new PCA9685Servo(device.getChannel(SERVO_SHIFT)); - VehiclePlatform vehicle = new VehiclePlatform(0, throttleEngine, steeringEngine, legEngine, shiftEngine); - vehicle.setThrottle(throttle); - vehicle.setSteering(steering); - vehicle.setLeg(leg); - vehicle.setShift(shift); - Thread.sleep(duration); - vehicle.setThrottle(0); - vehicle.setSteering(0); - vehicle.setLeg(0); - vehicle.setShift(0); - } + VehiclePlatform vehicle = new VehiclePlatform(0, throttleEngine, steeringEngine, legEngine, shiftEngine); + vehicle.setThrottle(throttle); + vehicle.setSteering(steering); + vehicle.setLeg(leg); + vehicle.setShift(shift); + Thread.sleep(duration); + vehicle.setThrottle(0); + vehicle.setSteering(0); + vehicle.setLeg(0); + vehicle.setShift(0); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PWMServoExample.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PWMServoExample.java index c66043f3..37499be8 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PWMServoExample.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/PWMServoExample.java @@ -17,27 +17,31 @@ package com.robo4j.hw.rpi.pwm; import com.robo4j.hw.rpi.utils.GpioPin; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; /** * This example assumes a servo connected to the GPIO_01 hardware PWM pin. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class PWMServoExample { - public static void main(String[] args) throws IOException, - InterruptedException { - if (args.length != 1) { - System.out.println("Usage: PWMExample "); - System.exit(2); - } - float input = Float.parseFloat(args[0]); - PWMServo servo = new PWMServo(GpioPin.GPIO_01, false); - System.out.println("Setting input to " + input); - servo.setInput(input); - System.out.println("Press to quit"); - System.in.read(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(PWMServoExample.class); + + public static void main(String[] args) throws IOException, + InterruptedException { + if (args.length != 1) { + LOGGER.info("Usage: PWMExample "); + System.exit(2); + } + float input = Float.parseFloat(args[0]); + PWMServo servo = new PWMServo(GpioPin.GPIO_01, false); + LOGGER.info("Setting input to {}", input); + servo.setInput(input); + LOGGER.info("Press to quit"); + System.in.read(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTankTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTankTest.java index aaef6c38..6e3c1467 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTankTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTankTest.java @@ -16,51 +16,55 @@ */ package com.robo4j.hw.rpi.pwm.roboclaw; -import java.io.IOException; - import com.robo4j.hw.rpi.Servo; import com.robo4j.hw.rpi.i2c.pwm.PCA9685Servo; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * This example assumes two servos connected to specific channels (6 and 7). It * is important to modify this example to match your setup. - * + * * This example should be modified to suit your setup! - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class RoboClawRCTankTest { - // The internetz says 50Hz is the standard PWM frequency for operating RC - // servos. - private static final int SERVO_FREQUENCY = 50; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboClawRCTankTest.class); + // The internetz says 50Hz is the standard PWM frequency for operating RC + // servos. + private static final int SERVO_FREQUENCY = 50; - public static void main(String[] args) throws IOException, InterruptedException { - if (args.length != 3) { - System.out.println("Usage: Tank "); - System.out.flush(); - System.exit(2); - } - float speed = Float.parseFloat(args[0]); - float direction = (float) Math.toRadians(Float.parseFloat(args[1])); - int duration = Integer.parseInt(args[2]); + public static void main(String[] args) throws IOException, InterruptedException { + if (args.length != 3) { + LOGGER.info("Usage: Tank "); + System.out.flush(); + System.exit(2); + } + float speed = Float.parseFloat(args[0]); + float direction = (float) Math.toRadians(Float.parseFloat(args[1])); + int duration = Integer.parseInt(args[2]); - testEngine(speed, direction, duration); - System.out.println("All done! Bye!"); - } + testEngine(speed, direction, duration); + LOGGER.info("All done! Bye!"); + } - public static void testEngine(float speed, float direction, int duration) throws IOException, InterruptedException { - System.out.printf("Running for %d ms with speed %f and direction %f.%n", duration, speed, direction); - PWMPCA9685Device device = new PWMPCA9685Device(); - device.setPWMFrequency(SERVO_FREQUENCY); - Servo leftEngine = new PCA9685Servo(device.getChannel(6)); - Servo rightEngine = new PCA9685Servo(device.getChannel(7)); + public static void testEngine(float speed, float direction, int duration) throws IOException, InterruptedException { + // TODO: add measurement units + LOGGER.info("duration:{}ms with speed:{} and direction:{}", duration, speed, direction); + PWMPCA9685Device device = new PWMPCA9685Device(); + device.setPWMFrequency(SERVO_FREQUENCY); + Servo leftEngine = new PCA9685Servo(device.getChannel(6)); + Servo rightEngine = new PCA9685Servo(device.getChannel(7)); - RoboClawRCTank tank = new RoboClawRCTank(leftEngine, rightEngine); - tank.setDirection(direction); - tank.setSpeed(speed); - Thread.sleep(duration); - tank.setSpeed(0); - } + RoboClawRCTank tank = new RoboClawRCTank(leftEngine, rightEngine); + tank.setDirection(direction); + tank.setSpeed(speed); + Thread.sleep(duration); + tank.setSpeed(0); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/gps/GPSTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/gps/GPSTest.java index 9c6abde3..db8d399a 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/gps/GPSTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/gps/GPSTest.java @@ -16,37 +16,40 @@ */ package com.robo4j.hw.rpi.serial.gps; -import java.io.IOException; - import com.robo4j.hw.rpi.gps.GPS; import com.robo4j.hw.rpi.gps.GPSListener; import com.robo4j.hw.rpi.gps.PositionEvent; import com.robo4j.hw.rpi.gps.VelocityEvent; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Listens for GPS event and prints them to stdout as they come. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GPSTest { - // TODO : review duplicates - public static void main(String[] args) throws InterruptedException, IOException { - GPS mtk3339gps = new MTK3339GPS(); - mtk3339gps.addListener(new GPSListener() { - @Override - public void onPosition(PositionEvent event) { - System.out.println(event); - } + private static final Logger LOGGER = LoggerFactory.getLogger(GPSTest.class); + + public static void main(String[] args) throws InterruptedException, IOException { + GPS mtk3339gps = new MTK3339GPS(); + mtk3339gps.addListener(new GPSListener() { + @Override + public void onPosition(PositionEvent event) { + LOGGER.info("onPosition, event:{}", event); + } - @Override - public void onVelocity(VelocityEvent event) { - System.out.println(event); - } - }); - mtk3339gps.start(); - System.out.println("Press to quit!"); - System.in.read(); - mtk3339gps.shutdown(); - } + @Override + public void onVelocity(VelocityEvent event) { + LOGGER.info("onVelocity, event:{}", event); + } + }); + mtk3339gps.start(); + LOGGER.info("Press to quit!"); + System.in.read(); + mtk3339gps.shutdown(); + } } diff --git a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarTest.java b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarTest.java index 5207812d..d8096fbd 100644 --- a/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarTest.java +++ b/robo4j-hw-rpi/src/examples/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarTest.java @@ -16,46 +16,40 @@ */ package com.robo4j.hw.rpi.serial.ydlidar; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.util.concurrent.TimeoutException; -//import com.pi4j.io.serial.SerialFactory; -import com.robo4j.hw.rpi.serial.ydlidar.ScanReceiver; -import com.robo4j.hw.rpi.serial.ydlidar.YDLidarDevice; -import com.robo4j.math.geometry.ScanResult2D; - /** * Example. Gets some information from the lidar, prints it, and then captures * data for 10 seconds. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class YDLidarTest { - public static void main(String[] args) throws IOException, InterruptedException, TimeoutException { - YDLidarDevice device = new YDLidarDevice(new ScanReceiver() { - @Override - public void onScan(ScanResult2D scanResult) { - System.out.println("Got scan result: " + scanResult); - } - }); - System.out.println("Restarting the device to make sure we start with a clean slate"); - device.restart(); - // Takes some serious time to restart this thing ;) - System.out.println("Waiting 4s for the device to properly boot up"); - Thread.sleep(4000); - System.out.println(device); - System.out.println(device.getDeviceInfo()); - System.out.println(device.getHealthInfo()); - System.out.println("Ranging Frequency = " + device.getRangingFrequency()); - System.out.println("Will capture data for 10 seconds..."); - device.setScanning(true); - Thread.sleep(10000); - device.setScanning(false); - device.shutdown(); - System.out.println("Done!"); - // Naughty that this has to be done... Perhaps fix Pi4J? -// SerialFactory.shutdown(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(YDLidarTest.class); + + //TODO: review example with sleep + public static void main(String[] args) throws IOException, InterruptedException, TimeoutException { + YDLidarDevice device = new YDLidarDevice(scanResult -> LOGGER.info("Got scan result:{}", scanResult)); + LOGGER.info("Restarting the device to make sure we start with a clean slate"); + device.restart(); + // Takes some serious time to restart this thing ;) + LOGGER.info("Waiting 4s for the device to properly boot up"); + Thread.sleep(4000); + LOGGER.info("device:{}", device); + LOGGER.info("deviceInfo:{}", device.getDeviceInfo()); + LOGGER.info("deviceHealth:{}", device.getHealthInfo()); + LOGGER.info("Ranging Frequency:{}", device.getRangingFrequency()); + LOGGER.info("Will capture data for 10 seconds..."); + device.setScanning(true); + Thread.sleep(10000); + device.setScanning(false); + device.shutdown(); + LOGGER.info("Done!"); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/AbstractI2CDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/AbstractI2CDevice.java index 643a31d8..e107c30b 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/AbstractI2CDevice.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/AbstractI2CDevice.java @@ -20,13 +20,14 @@ import com.pi4j.exception.InitializeException; import com.pi4j.io.i2c.I2C; import com.pi4j.io.i2c.I2CConfig; -import com.pi4j.plugin.linuxfs.LinuxFsPlugin; import com.pi4j.plugin.linuxfs.provider.i2c.LinuxFsI2CProviderImpl; +import com.robo4j.hw.rpi.i2c.magnetometer.MagnetometerLSM303Device; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.concurrent.TimeUnit; -import java.util.logging.Logger; /** * Abstract super class for I2C devices. @@ -35,6 +36,7 @@ * @author Miro Wengner (@miragemiko) */ public abstract class AbstractI2CDevice { + private static final Logger LOGGER = LoggerFactory.getLogger(AbstractI2CDevice.class); private static final String PROVIDER_NAME = "linuxfs-i2c"; private final I2cBus bus; private final int address; @@ -69,7 +71,7 @@ public AbstractI2CDevice(I2cBus bus, int address) throws IOException { var provider = new LinuxFsI2CProviderImpl(); provider.initialize(pi4jRpiContext); this.i2C = provider.create(i2CConfig); - System.out.println("create i2c device"); + LOGGER.info("create i2c device"); } catch (InitializeException e) { throw new IOException("Unsupported i2c config", e); } @@ -93,15 +95,6 @@ public final int getAddress() { return address; } - /** - * Convenience method to get a logger for the specific class. - * - * @return a logger for this class. - */ - public Logger getLogger() { - return Logger.getLogger(this.getClass().getName()); - } - /** * Writes the bytes directly to the I2C device. * diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/CalibratedFloat3DDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/CalibratedFloat3DDevice.java index 7f6f9f87..831e1f05 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/CalibratedFloat3DDevice.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/CalibratedFloat3DDevice.java @@ -16,46 +16,49 @@ */ package com.robo4j.hw.rpi.i2c; -import java.io.IOException; - import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Wrapper class for readable devices returning Float3D, allowing for calibration. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class CalibratedFloat3DDevice implements ReadableDevice { - private final Tuple3f centerOffsets; - private final Tuple3f rangeMultipliers; - private final ReadableDevice device; - - public CalibratedFloat3DDevice(ReadableDevice device, Tuple3f offsets, Tuple3f multipliers) { - this.device = device; - this.centerOffsets = offsets; - this.rangeMultipliers = multipliers; - } - - public Tuple3f read() throws IOException { - Tuple3f value = device.read(); - value.add(centerOffsets); - value.multiply(rangeMultipliers); - return value; - } - - public void setCalibration(Tuple3f offsets, Tuple3f multipliers) { - centerOffsets.set(offsets); - rangeMultipliers.set(multipliers); - System.out.println("Gyro offsets: " + centerOffsets); - System.out.println("Gyro multipliers: " + rangeMultipliers); - } - - protected Tuple3f getRangeMultipliers() { - return rangeMultipliers; - } - - protected Tuple3f getCenterOffsets() { - return centerOffsets; - } + private static final Logger LOGGER = LoggerFactory.getLogger(CalibratedFloat3DDevice.class); + private final Tuple3f centerOffsets; + private final Tuple3f rangeMultipliers; + private final ReadableDevice device; + + public CalibratedFloat3DDevice(ReadableDevice device, Tuple3f offsets, Tuple3f multipliers) { + this.device = device; + this.centerOffsets = offsets; + this.rangeMultipliers = multipliers; + } + + public Tuple3f read() throws IOException { + Tuple3f value = device.read(); + value.add(centerOffsets); + value.multiply(rangeMultipliers); + return value; + } + + public void setCalibration(Tuple3f offsets, Tuple3f multipliers) { + centerOffsets.set(offsets); + rangeMultipliers.set(multipliers); + LOGGER.debug("Gyro offsets: {}", centerOffsets); + LOGGER.debug("Gyro multipliers: {}", rangeMultipliers); + } + + protected Tuple3f getRangeMultipliers() { + return rangeMultipliers; + } + + protected Tuple3f getCenterOffsets() { + return centerOffsets; + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Device.java index f13ba745..bac90ce2 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Device.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/accelerometer/AccelerometerLSM303Device.java @@ -16,143 +16,146 @@ */ package com.robo4j.hw.rpi.i2c.accelerometer; -import java.io.IOException; - import com.robo4j.hw.rpi.i2c.AbstractI2CDevice; import com.robo4j.hw.rpi.i2c.ReadableDevice; import com.robo4j.hw.rpi.utils.I2cBus; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Abstraction for reading data from a LSM303 accelerometer, for example the one * on the Adafruit IMU breakout board. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class AccelerometerLSM303Device extends AbstractI2CDevice implements ReadableDevice { - public static final float GRAVITY_ZURICH = 9.807f; - - public static final int AXIS_ENABLE_X = 1; - public static final int AXIS_ENABLE_Y = 2; - public static final int AXIS_ENABLE_Z = 4; - public static final int AXIS_ENABLE_ALL = AXIS_ENABLE_X | AXIS_ENABLE_Y | AXIS_ENABLE_Z; - - private static final int DEFAULT_I2C_ADDRESS = 0x19; - private static final int CTRL_REG1_A = 0x20; - private static final int CTRL_REG4_A = 0x23; - private static final int OUT_X_L_A = 0x28; - - private static final int HIGH_RES_ENABLE = 0x8; - private static final int HIGH_RES_DISABLE = 0x0; - - private final FullScale scale; - - public enum DataRate { - POWER_DOWN(0x0), HZ_1(0x10), HZ_10(0x20), HZ_25(0x30), HZ_50(0x40), HZ_100(0x50), HZ_200(0x60), HZ_400( - 0x70), HZ_LP_1620(0x80), HZ_N_1354_LP_5376(0x81); - - private int ctrlCode; - - DataRate(int ctrlCode) { - this.ctrlCode = ctrlCode; - } - - public int getCtrlCode() { - return ctrlCode; - } - } - - public enum PowerMode { - NORMAL(0x0), LOW_POWER(0x8); - - private int ctrlCode; - - PowerMode(int ctrlCode) { - this.ctrlCode = ctrlCode; - } - - public int getCtrlCode() { - return ctrlCode; - } - } - - public enum FullScale { - G_2(0x0, 1), G_4(0x10, 1), G_8(0x20, 4), G_16(0x30, 12); - - private int ctrlCode; - private int sensitivity; - - FullScale(int ctrlCode, int sensitivity) { - this.ctrlCode = ctrlCode; - this.sensitivity = sensitivity; - } - - public int getCtrlCode() { - return ctrlCode; - } - - /** - * @return the sensitivity in mg/LSB. - */ - public int getSensitivity() { - return sensitivity; - } - } - - public AccelerometerLSM303Device() throws IOException { - this(PowerMode.NORMAL, DataRate.HZ_10, FullScale.G_2, false); - } - - public AccelerometerLSM303Device(PowerMode mode, DataRate rate, FullScale scale, boolean highres) - throws IOException { - this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, mode, rate, AXIS_ENABLE_ALL, scale, highres); - } - - public AccelerometerLSM303Device(I2cBus bus, int address, PowerMode mode, DataRate rate, int axisEnable, - FullScale scale, boolean highres) throws IOException { - super(bus, address); - this.scale = scale; - initialize(mode, rate, axisEnable, scale, highres); - } - - /** - * @return current acceleration, m/s^2 - * @throws IOException - * exception - */ - public synchronized Tuple3f read() throws IOException { - Tuple3f rawData = new Tuple3f(); - byte[] data = new byte[6]; - // int n = i2CConfig.read(OUT_X_L_A | 0x80, data, 0, 6); - int n = readBufferByAddress(OUT_X_L_A | 0x80, data, 0, 6); - if (n != 6) { - getLogger().warning("Failed to read all data from accelerometer. Should have read 6, could only read " + n); - } - float k = scale.getSensitivity() / 1000.0f; - rawData.x = read12bitSigned(data, 0) * k; - rawData.y = read12bitSigned(data, 2) * k; - rawData.z = read12bitSigned(data, 4) * k; - return rawData; - } - - private int read12bitSigned(byte[] data, int offset) { - short val = (short) ((data[offset + 1] & 0xFF) << 8 | (data[offset] & 0xFF)); - return val >> 4; - - } - - private void initialize(PowerMode mode, DataRate rate, int axisEnable, FullScale scale, boolean highres) - throws IOException { - byte config = (byte) ((mode.getCtrlCode() | rate.getCtrlCode() | axisEnable) & 0xFF); - writeByte(CTRL_REG1_A, config); - byte check = (byte) readByte(CTRL_REG1_A); - if (config != check) { - throw new IOException("Could not properly initialize accelerometer"); - } - int hfact = highres ? HIGH_RES_ENABLE : HIGH_RES_DISABLE; - config = (byte) ((scale.getCtrlCode() | hfact) & 0xFF); - System.out.println(config); - writeByte(CTRL_REG4_A, config); - } + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerLSM303Device.class); + public static final float GRAVITY_ZURICH = 9.807f; + + public static final int AXIS_ENABLE_X = 1; + public static final int AXIS_ENABLE_Y = 2; + public static final int AXIS_ENABLE_Z = 4; + public static final int AXIS_ENABLE_ALL = AXIS_ENABLE_X | AXIS_ENABLE_Y | AXIS_ENABLE_Z; + + private static final int DEFAULT_I2C_ADDRESS = 0x19; + private static final int CTRL_REG1_A = 0x20; + private static final int CTRL_REG4_A = 0x23; + private static final int OUT_X_L_A = 0x28; + + private static final int HIGH_RES_ENABLE = 0x8; + private static final int HIGH_RES_DISABLE = 0x0; + + private final FullScale scale; + + public enum DataRate { + POWER_DOWN(0x0), HZ_1(0x10), HZ_10(0x20), HZ_25(0x30), HZ_50(0x40), HZ_100(0x50), HZ_200(0x60), HZ_400( + 0x70), HZ_LP_1620(0x80), HZ_N_1354_LP_5376(0x81); + + private int ctrlCode; + + DataRate(int ctrlCode) { + this.ctrlCode = ctrlCode; + } + + public int getCtrlCode() { + return ctrlCode; + } + } + + public enum PowerMode { + NORMAL(0x0), LOW_POWER(0x8); + + private int ctrlCode; + + PowerMode(int ctrlCode) { + this.ctrlCode = ctrlCode; + } + + public int getCtrlCode() { + return ctrlCode; + } + } + + public enum FullScale { + G_2(0x0, 1), G_4(0x10, 1), G_8(0x20, 4), G_16(0x30, 12); + + private int ctrlCode; + private int sensitivity; + + FullScale(int ctrlCode, int sensitivity) { + this.ctrlCode = ctrlCode; + this.sensitivity = sensitivity; + } + + public int getCtrlCode() { + return ctrlCode; + } + + /** + * @return the sensitivity in mg/LSB. + */ + public int getSensitivity() { + return sensitivity; + } + } + + public AccelerometerLSM303Device() throws IOException { + this(PowerMode.NORMAL, DataRate.HZ_10, FullScale.G_2, false); + } + + public AccelerometerLSM303Device(PowerMode mode, DataRate rate, FullScale scale, boolean highres) + throws IOException { + this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, mode, rate, AXIS_ENABLE_ALL, scale, highres); + } + + public AccelerometerLSM303Device(I2cBus bus, int address, PowerMode mode, DataRate rate, int axisEnable, + FullScale scale, boolean highres) throws IOException { + super(bus, address); + this.scale = scale; + initialize(mode, rate, axisEnable, scale, highres); + } + + /** + * @return current acceleration, m/s^2 + * @throws IOException exception + */ + public synchronized Tuple3f read() throws IOException { + Tuple3f rawData = new Tuple3f(); + byte[] data = new byte[6]; + // int n = i2CConfig.read(OUT_X_L_A | 0x80, data, 0, 6); + int n = readBufferByAddress(OUT_X_L_A | 0x80, data, 0, 6); + if (n != 6) { + LOGGER.warn("Failed to read all data from accelerometer. Should have read 6, could only read {}", n); + } + float k = scale.getSensitivity() / 1000.0f; + rawData.x = read12bitSigned(data, 0) * k; + rawData.y = read12bitSigned(data, 2) * k; + rawData.z = read12bitSigned(data, 4) * k; + return rawData; + } + + private int read12bitSigned(byte[] data, int offset) { + short val = (short) ((data[offset + 1] & 0xFF) << 8 | (data[offset] & 0xFF)); + return val >> 4; + + } + + private void initialize(PowerMode mode, DataRate rate, int axisEnable, FullScale scale, boolean highres) + throws IOException { + byte config = (byte) ((mode.getCtrlCode() | rate.getCtrlCode() | axisEnable) & 0xFF); + writeByte(CTRL_REG1_A, config); + byte check = (byte) readByte(CTRL_REG1_A); + if (config != check) { + throw new IOException("Could not properly initialize accelerometer"); + } + int hfact = highres ? HIGH_RES_ENABLE : HIGH_RES_DISABLE; + config = (byte) ((scale.getCtrlCode() | hfact) & 0xFF); + LOGGER.debug("initialize, config:{}", config); + LOGGER.debug("config:{}", config); + writeByte(CTRL_REG4_A, config); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AbstractBackpack.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AbstractBackpack.java index 3cda4639..1e14d227 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AbstractBackpack.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/AbstractBackpack.java @@ -17,204 +17,191 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; -import java.io.IOException; - import com.robo4j.hw.rpi.i2c.AbstractI2CDevice; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * AbstractBackpack is the abstraction for all Adafruit Backpack devices * https://learn.adafruit.com/adafruit-led-backpack/overview * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public abstract class AbstractBackpack extends AbstractI2CDevice { - - public static final int DEFAULT_BRIGHTNESS = 15; - private static final int DEFAULT_ADDRESS = 0x70; - private static final int OSCILLATOR_TURN_ON = 0x21; - private static final int HT16K33_BLINK_CMD = 0x80; - private static final int HT16K33_BLINK_DISPLAY_ON = 0x01; - private static final int HT16K33_CMD_BRIGHTNESS = 0xE0; - private static final int HT16K33_BLINK_OFF = 0x00; - private final short[] buffer = new short[8]; // uint16_t - - AbstractBackpack() throws IOException { - this(I2cBus.BUS_1, DEFAULT_ADDRESS, DEFAULT_BRIGHTNESS); - } - - AbstractBackpack(I2cBus bus, int address, int brightness) throws IOException { - super(bus, address); - initiate(brightness); - } - - public void display() { - try { - writeDisplay(); - } catch (IOException e) { - System.out.printf("error display: %s%n", e.getMessage()); - } - } - - public void clear() { - try { - clearBuffer(); - } catch (IOException e) { - System.out.printf("error clear: %s%n", e.getMessage()); - } - } - - short intToShort(int value) { - return (short) (value & 0xFFFF); - } - - /** - * turn on of the position on the matrix grid - * - * @param x - * x position on the matrix - * @param y - * y position on the matrix - * @param color - * led color - */ - void setColorByMatrixToBuffer(short x, short y, BiColor color) { - switch (color) { - case RED: - // Turn on red LED. - buffer[y] |= _BV(intToShort(x + 8)); - // Turn off green LED. - buffer[y] &= ~_BV(x); - break; - case YELLOW: - // Turn on green and red LED. - buffer[y] |= _BV(intToShort(x + 8)) | _BV(x); - break; - case GREEN: - // Turn on green LED. - buffer[y] |= _BV(x); - // Turn off red LED. - buffer[y] &= ~_BV(intToShort(x + 8)); - break; - case OFF: - buffer[y] &= ~_BV(x) & ~_BV(intToShort(x + 8)); - break; - default: - System.out.println("setColorByMatrixToBuffer: " + color); - break; - } - } - - /** - * - * @param n - * position on alphanumeric display - * @param c - * character to be displayed - * @param dp - * the dot next to the character - */ - void setCharacter(int n, int c, boolean dp) { - short value = (short) c; - setValue(n, value, dp); - } - - /** - * - * @param n - * position - * @param v - * value 16-bits - * @param dp - * the dot next to the character - */ - void setValue(int n, short v, boolean dp) { - buffer[n] = dp ? (v |= (1 << 14)) : v; - } - - /** - * Turn off/on the a led on the bar - * - * @param a - * position on the bar - * @param c - * position on the bar - * @param color - * color on the bar - */ - void setColorToBarBuffer(short a, short c, BiColor color) { - switch (color) { - case RED: - // Turn on red LED. - buffer[c] |= _BV(a); - // Turn off green LED. - buffer[c] &= ~_BV(intToShort(a + 8)); - break; - case YELLOW: - // Turn on red and green LED. - buffer[c] |= _BV(a) | _BV(intToShort(a + 8)); - break; - case GREEN: - // Turn on green LED. - buffer[c] |= _BV(intToShort(a + 8)); - // Turn off red LED. - buffer[c] &= ~_BV(a); - break; - case OFF: - // Turn off red and green LED. - buffer[c] &= ~_BV(a) & ~_BV(intToShort(a + 8)); - break; - default: - System.out.println("setColorToBarBuffer: " + color); - break; - } - } - - private void initiate(int brightness) throws IOException { + private static final Logger LOGGER = LoggerFactory.getLogger(AbstractBackpack.class); + public static final int DEFAULT_BRIGHTNESS = 15; + private static final int DEFAULT_ADDRESS = 0x70; + private static final int OSCILLATOR_TURN_ON = 0x21; + private static final int HT16K33_BLINK_CMD = 0x80; + private static final int HT16K33_BLINK_DISPLAY_ON = 0x01; + private static final int HT16K33_CMD_BRIGHTNESS = 0xE0; + private static final int HT16K33_BLINK_OFF = 0x00; + private final short[] buffer = new short[8]; // uint16_t + + AbstractBackpack() throws IOException { + this(I2cBus.BUS_1, DEFAULT_ADDRESS, DEFAULT_BRIGHTNESS); + } + + AbstractBackpack(I2cBus bus, int address, int brightness) throws IOException { + super(bus, address); + initiate(brightness); + } + + public void display() { + try { + writeDisplay(); + } catch (IOException e) { + LOGGER.error("error display:{}", e.getMessage()); + } + } + + public void clear() { + try { + clearBuffer(); + } catch (IOException e) { + LOGGER.error("error clear:{}", e.getMessage()); + } + } + + short intToShort(int value) { + return (short) (value & 0xFFFF); + } + + /** + * turn on of the position on the matrix grid + * + * @param x x position on the matrix + * @param y y position on the matrix + * @param color led color + */ + void setColorByMatrixToBuffer(short x, short y, BiColor color) { + switch (color) { + case RED: + // Turn on red LED. + buffer[y] |= _BV(intToShort(x + 8)); + // Turn off green LED. + buffer[y] &= ~_BV(x); + break; + case YELLOW: + // Turn on green and red LED. + buffer[y] |= _BV(intToShort(x + 8)) | _BV(x); + break; + case GREEN: + // Turn on green LED. + buffer[y] |= _BV(x); + // Turn off red LED. + buffer[y] &= ~_BV(intToShort(x + 8)); + break; + case OFF: + buffer[y] &= ~_BV(x) & ~_BV(intToShort(x + 8)); + break; + default: + LOGGER.warn("setColorByMatrixToBuffer: {}", color); + break; + } + } + + /** + * @param n position on alphanumeric display + * @param c character to be displayed + * @param dp the dot next to the character + */ + void setCharacter(int n, int c, boolean dp) { + short value = (short) c; + setValue(n, value, dp); + } + + /** + * @param n position + * @param v value 16-bits + * @param dp the dot next to the character + */ + void setValue(int n, short v, boolean dp) { + buffer[n] = dp ? (v |= (1 << 14)) : v; + } + + /** + * Turn off/on the a led on the bar + * + * @param a position on the bar + * @param c position on the bar + * @param color color on the bar + */ + void setColorToBarBuffer(short a, short c, BiColor color) { + switch (color) { + case RED: + // Turn on red LED. + buffer[c] |= _BV(a); + // Turn off green LED. + buffer[c] &= ~_BV(intToShort(a + 8)); + break; + case YELLOW: + // Turn on red and green LED. + buffer[c] |= _BV(a) | _BV(intToShort(a + 8)); + break; + case GREEN: + // Turn on green LED. + buffer[c] |= _BV(intToShort(a + 8)); + // Turn off red LED. + buffer[c] &= ~_BV(a); + break; + case OFF: + // Turn off red and green LED. + buffer[c] &= ~_BV(a) & ~_BV(intToShort(a + 8)); + break; + default: + LOGGER.warn("setColorToBarBuffer: {}", color); + break; + } + } + + private void initiate(int brightness) throws IOException { // i2CConfig.write((byte) (OSCILLATOR_TURN_ON)); // Turn on oscilator // i2CConfig.write(blinkRate(HT16K33_BLINK_OFF)); // i2CConfig.write(setBrightness(brightness)); - writeByte((byte) (OSCILLATOR_TURN_ON)); // Turn on oscilator - writeByte(blinkRate(HT16K33_BLINK_OFF)); - writeByte(setBrightness(brightness)); - } - - private void writeDisplay() throws IOException { - int address = 0; - for (int i = 0; i < buffer.length; i++) { + writeByte((byte) (OSCILLATOR_TURN_ON)); // Turn on oscilator + writeByte(blinkRate(HT16K33_BLINK_OFF)); + writeByte(setBrightness(brightness)); + } + + private void writeDisplay() throws IOException { + int address = 0; + for (int i = 0; i < buffer.length; i++) { // i2CConfig.write(address++, (byte) (buffer[i] & 0xFF)); // i2CConfig.write(address++, (byte) (buffer[i] >> 8)); - writeByte(address++, (byte) (buffer[i] & 0xFF)); - writeByte(address++, (byte) (buffer[i] >> 8)); - } - } - - private void clearBuffer() throws IOException { - for (int i = 0; i < buffer.length; i++) { - buffer[i] = 0; - } - } - - private short _BV(short i) { - return ((short) (1 << i)); - } - - private byte setBrightness(int b) { - if (b > 15) - b = 15; - return uint8ToByte(HT16K33_CMD_BRIGHTNESS | b); - } - - private byte blinkRate(int b) { - if (b > 3) - b = 0; - return uint8ToByte(HT16K33_BLINK_CMD | HT16K33_BLINK_DISPLAY_ON | (b << 1)); - } - - private byte uint8ToByte(int value) { - return (byte) (value & 0xFF); - } + writeByte(address++, (byte) (buffer[i] & 0xFF)); + writeByte(address++, (byte) (buffer[i] >> 8)); + } + } + + private void clearBuffer() throws IOException { + for (int i = 0; i < buffer.length; i++) { + buffer[i] = 0; + } + } + + private short _BV(short i) { + return ((short) (1 << i)); + } + + private byte setBrightness(int b) { + if (b > 15) + b = 15; + return uint8ToByte(HT16K33_CMD_BRIGHTNESS | b); + } + + private byte blinkRate(int b) { + if (b > 3) + b = 0; + return uint8ToByte(HT16K33_BLINK_CMD | HT16K33_BLINK_DISPLAY_ON | (b << 1)); + } + + private byte uint8ToByte(int value) { + return (byte) (value & 0xFF); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BarDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BarDevice.java index 93647315..33eabd7f 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BarDevice.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitbackpack/BiColor24BarDevice.java @@ -18,6 +18,8 @@ package com.robo4j.hw.rpi.i2c.adafruitbackpack; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -30,7 +32,7 @@ * @author Miroslav Wengner (@miragemiko) */ public class BiColor24BarDevice extends AbstractBackpack { - + private static final Logger LOGGER = LoggerFactory.getLogger(BiColor24BarDevice.class); public static final int MAX_BARS = 24; public BiColor24BarDevice(I2cBus bus, int address, int brightness) throws IOException { @@ -43,7 +45,7 @@ public BiColor24BarDevice() throws IOException { public void setBar(int bar, BiColor color) { if (!validatePosition(bar)) { - System.out.println("addBar: not allowed bar= %s" + bar); + LOGGER.debug("addBar: not allowed bar= %s{}", bar); return; } short a, c; diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/ButtonPressedObserver.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/ButtonPressedObserver.java index 08ea6231..d616fe27 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/ButtonPressedObserver.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/ButtonPressedObserver.java @@ -16,11 +16,12 @@ */ package com.robo4j.hw.rpi.i2c.adafruitlcd; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.util.LinkedList; import java.util.List; -import java.util.logging.Level; -import java.util.logging.Logger; /** * This class provides an example on how you can listen for buttons being @@ -30,103 +31,103 @@ * (pin 20) to the GPIO port and check for interrupts before reading the state * of the input pins. Note that such changes require an update to the LCD class * as well. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ //TODO, FIXME -> we should remove thread sleep public class ButtonPressedObserver { - private volatile boolean isRunning = false; - private final List buttonListeners = new LinkedList(); - private final AdafruitLcd lcd; - private final long [] buttonDownTimes = new long[Button.values().length]; - private volatile Thread t; - - private class ButtonChecker implements Runnable { - @Override - public void run() { - while (isRunning) { - try { - for (Button button : Button.values()) { - if (button.isButtonPressed(lcd.buttonsPressedBitmask())) { - if (buttonDownTimes[button.getPin()] != 0) { - continue; - } - buttonDownTimes[button.getPin()] = System.currentTimeMillis(); - } else if (buttonDownTimes[button.getPin()] != 0) { - if ((System.currentTimeMillis() - buttonDownTimes[button.getPin()]) > 15) { - fireNotification(button); - } - buttonDownTimes[button.getPin()] = 0; - } - } - } catch (IOException e) { - Logger.getLogger("se.hirt.pi.adafruit").log(Level.SEVERE, - "Could not get buttons bitmask!", e); - } - sleep(15); - Thread.yield(); - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(ButtonPressedObserver.class); + private volatile boolean isRunning = false; + private final List buttonListeners = new LinkedList(); + private final AdafruitLcd lcd; + private final long[] buttonDownTimes = new long[Button.values().length]; + private volatile Thread t; + + // TODO : review sleeps + private class ButtonChecker implements Runnable { + @Override + public void run() { + while (isRunning) { + try { + for (Button button : Button.values()) { + if (button.isButtonPressed(lcd.buttonsPressedBitmask())) { + if (buttonDownTimes[button.getPin()] != 0) { + continue; + } + buttonDownTimes[button.getPin()] = System.currentTimeMillis(); + } else if (buttonDownTimes[button.getPin()] != 0) { + if ((System.currentTimeMillis() - buttonDownTimes[button.getPin()]) > 15) { + fireNotification(button); + } + buttonDownTimes[button.getPin()] = 0; + } + } + } catch (IOException e) { + LOGGER.error("Could not get buttons bitmask!", e); + } + sleep(15); + Thread.yield(); + } + } - private void sleep(int time) { - try { - Thread.sleep(time); - } catch (InterruptedException e) { - Logger.getLogger("se.hirt.pi.adafruit").log(Level.SEVERE, - "Could not get buttons bitmask!", e); - } - } + private void sleep(int time) { + try { + Thread.sleep(time); + } catch (InterruptedException e) { + LOGGER.error("Could not get buttons bitmask!", e); + } + } - private void fireNotification(Button trackedButton) { - ButtonListener[] listeners; - synchronized (buttonListeners) { - listeners = buttonListeners - .toArray(new ButtonListener[buttonListeners.size()]); - } - for (ButtonListener l : listeners) { - l.onButtonPressed(trackedButton); - } - } + private void fireNotification(Button trackedButton) { + ButtonListener[] listeners; + synchronized (buttonListeners) { + listeners = buttonListeners + .toArray(new ButtonListener[buttonListeners.size()]); + } + for (ButtonListener l : listeners) { + l.onButtonPressed(trackedButton); + } + } - } + } - public ButtonPressedObserver(AdafruitLcd lcd) { - this.lcd = lcd; - } + public ButtonPressedObserver(AdafruitLcd lcd) { + this.lcd = lcd; + } - public void removeButtonListener(ButtonListener l) { - synchronized (buttonListeners) { - buttonListeners.remove(l); - if (buttonListeners.isEmpty()) { - stopButtonMonitor(); - } - } - } + public void removeButtonListener(ButtonListener l) { + synchronized (buttonListeners) { + buttonListeners.remove(l); + if (buttonListeners.isEmpty()) { + stopButtonMonitor(); + } + } + } - public void stopButtonMonitor() { - isRunning = false; - try { - t.join(2000); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } + public void stopButtonMonitor() { + isRunning = false; + try { + t.join(2000); + } catch (InterruptedException e) { + LOGGER.error("stopButtonMonitor", e); + } + } - public void addButtonListener(ButtonListener l) { - synchronized (buttonListeners) { - if (buttonListeners.isEmpty()) { - startButtonMonitor(); - } - buttonListeners.add(l); - } - } + public void addButtonListener(ButtonListener l) { + synchronized (buttonListeners) { + if (buttonListeners.isEmpty()) { + startButtonMonitor(); + } + buttonListeners.add(l); + } + } - //TODO, FIXME: do not randomly threads use executor - public void startButtonMonitor() { - isRunning = true; - t = new Thread(new ButtonChecker(), "Button Checker"); - t.setDaemon(true); - t.start(); - } + //TODO, FIXME: do not randomly threads use executor + public void startButtonMonitor() { + isRunning = true; + t = new Thread(new ButtonChecker(), "Button Checker"); + t.setDaemon(true); + t.start(); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java index bac4c496..79902ac2 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java @@ -17,6 +17,8 @@ package com.robo4j.hw.rpi.i2c.adafruitlcd; import com.pi4j.exception.InitializeException; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -25,11 +27,12 @@ * @author Miro Wengner (@miragemiko) */ public class Message { + private static final Logger LOGGER = LoggerFactory.getLogger(Message.class); private static final String COLOR_PREFIX = "-c"; public static void main(String[] params) throws IOException, InitializeException { if (params == null || params.length == 0) { - System.out.println("Usage: [-c] \nValid COLORNAME values are: OFF, RED, GREEN, BLUE, YELLOW, TEAL, VIOLET, WHITE, ON\n"); + LOGGER.info("Usage: [-c] \nValid COLORNAME values are: OFF, RED, GREEN, BLUE, YELLOW, TEAL, VIOLET, WHITE, ON\n"); } Color color = Color.GREEN; String message = ""; @@ -44,7 +47,7 @@ public static void main(String[] params) throws IOException, InitializeException lcd.setBacklight(color); message = massage(message); lcd.setText(message); - System.out.println(message); + LOGGER.debug(message); } private static String massage(String message) { diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java index 864b7a29..f169c84c 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java @@ -22,6 +22,8 @@ import com.robo4j.hw.rpi.i2c.AbstractI2CDevice; import com.robo4j.hw.rpi.utils.GpioPin; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.awt.*; import java.awt.image.BufferedImage; @@ -31,57 +33,59 @@ /** * Support for SSD1306 devices over I2C. A good example is the Adafruit 128x64 * (or 128x32) monochrome OLED. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class SSD1306Device extends AbstractI2CDevice { - private static final int DEFAULT_I2C_ADDRESS = 0x3c; - private static final byte CHARGE_PUMP_VALUE_ENABLE = 0x14; - private static final byte CHARGE_PUMP_VALUE_DISABLE = 0x10; - private static final int DEFAULT_CONTRAST = 0x88; - - private final BufferedImage image; - //private final GpioController gpio = GpioFactory.getInstance(); - //private final GpioPinDigitalOutput resetPin; - private final DigitalOutput gpioResetPin; - private final boolean useExternalVCC; - private final OLEDVariant oledType; - - public enum OLEDVariant { - Type96x16(96, 16, 0x2, 1), Type128x32(128, 32, 0x2, 3), Type128x64(128, 64, 0x12, 7); - - private final int width; - private final int height; - private final int comPins; - private final int pageEnd; - - OLEDVariant(int width, int height, int comPins, int pageEnd) { - this.width = width; - this.height = height; - this.comPins = comPins; - this.pageEnd = pageEnd; - } - - public int getWidth() { - return width; - } - - public int getHeight() { - return height; - } - - public int getComPins() { - return comPins; - } - - public int getPageEnd() { - return pageEnd; - } - } - - private enum Commands { - //@formatter:off + private static final Logger LOGGER = LoggerFactory.getLogger(SSD1306Device.class); + + private static final int DEFAULT_I2C_ADDRESS = 0x3c; + private static final byte CHARGE_PUMP_VALUE_ENABLE = 0x14; + private static final byte CHARGE_PUMP_VALUE_DISABLE = 0x10; + private static final int DEFAULT_CONTRAST = 0x88; + + private final BufferedImage image; + //private final GpioController gpio = GpioFactory.getInstance(); + //private final GpioPinDigitalOutput resetPin; + private final DigitalOutput gpioResetPin; + private final boolean useExternalVCC; + private final OLEDVariant oledType; + + public enum OLEDVariant { + Type96x16(96, 16, 0x2, 1), Type128x32(128, 32, 0x2, 3), Type128x64(128, 64, 0x12, 7); + + private final int width; + private final int height; + private final int comPins; + private final int pageEnd; + + OLEDVariant(int width, int height, int comPins, int pageEnd) { + this.width = width; + this.height = height; + this.comPins = comPins; + this.pageEnd = pageEnd; + } + + public int getWidth() { + return width; + } + + public int getHeight() { + return height; + } + + public int getComPins() { + return comPins; + } + + public int getPageEnd() { + return pageEnd; + } + } + + private enum Commands { + //@formatter:off DISPLAY_OFF((byte) 0xae), DISPLAY_ON((byte) 0xaf), INVERTED_ON((byte) 0xa7), @@ -106,212 +110,195 @@ private enum Commands { SET_COLUMN_ADDRESS((byte) 0x21), SET_PAGE_ADDRESS((byte) 0x22); //@formatter:on - private final byte commandValue; - - Commands(byte commandValue) { - this.commandValue = commandValue; - } - - public byte getCommandValue() { - return commandValue; - } - } - - private enum MemoryModes { - HORIZONTAL((byte) 0), VERTICAL((byte) 1), PAGE((byte) 2); - - private byte value; - - MemoryModes(byte value) { - this.value = value; - } - - public byte getValue() { - return value; - } - - } - - /** - * Constructor. - * - * @param variant - * the oled variant, most commonly the 32 or 64 line version. - * @param resetPin - * the GPIO pin used for the reset. - * - * @throws IOException - * if there was a communication problem. - */ - public SSD1306Device(OLEDVariant variant, GpioPin resetPin) throws IOException { - this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, variant, resetPin, false); - } - - /** - * Constructor. - * - * @param bus - * the I2C bus used. - * @param address - * the I2C address of the OLED device, most commonly 0x3c. - * @param oledType - * the variant of the oled (depends on which version you own). - * @param resetPinId - * the GPIO pin used for reset (depends on your wiring). - * @param useExternalVCC - * use external VCC to drive the OLED. If false, the internal - * charge pump will be used to regulate to the necessary voltage. - * This is most commonly false. - * - * @throws IOException - * if there was a communication problem. - */ - public SSD1306Device(I2cBus bus, int address, OLEDVariant oledType, GpioPin resetPinId, boolean useExternalVCC) throws IOException { - super(bus, address); - this.image = new BufferedImage(oledType.getWidth(), oledType.getHeight(), BufferedImage.TYPE_BYTE_BINARY); - - var pi4jRpiContext = Pi4J.newAutoContext(); - var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); - var gpioResetPinConfig = digitalOutputBuilder.address(resetPinId.address()).onState(DigitalState.HIGH).build(); - - this.gpioResetPin = pi4jRpiContext.dout().create(gpioResetPinConfig); - - //this.resetPin = gpio.provisionDigitalOutputPin(resetPinId, "reset", PinState.HIGH); - this.useExternalVCC = useExternalVCC; - this.oledType = oledType; - initialize(); - } - - /** - * @return the graphics context upon which to draw. This being a monochrome - * display, only the colors {@link Color}.black and {@link Color} - * .white should be used. - */ - public Graphics2D getGraphicsContext() { - return image.createGraphics(); - } - - /** - * Pushes the image data to the device over I2C. - * - * @throws IOException - * exception - */ - public void pushImage() throws IOException { - executeCommand(Commands.SET_COLUMN_ADDRESS, 0, oledType.getWidth() - 1); - executeCommand(Commands.SET_PAGE_ADDRESS, 0, oledType.getPageEnd()); - - // Transmitting image data in one write - byte[] byteArray = toByteArray(); - System.out.println(Arrays.toString(byteArray)); - // TODO : const - writeByteBufferByAddress(0x40, byteArray); - } - - /** - * @param enable - * false to disable (turn off), true to enable (turn on). - * - * @throws IOException - * exception - */ - public void setEnabled(boolean enable) throws IOException { - if (enable) { - executeCommand(Commands.DISPLAY_ON); - } else { - executeCommand(Commands.DISPLAY_OFF); - } - } - - /** - * Sets the contrast between 0 (minimum) and 1.0 (max). - * - * @param contrast - * a value between 0 and 1.0. - * @throws IOException - * exception - */ - public void setContrast(float contrast) throws IOException { - executeCommand(Commands.SET_CONTRAST, Math.max(Math.round(contrast * 0xff), 0xff)); - } - - /** - * @return the image used to draw upon. - */ - public BufferedImage getImage() { - return image; - } - - private byte[] toByteArray() { - int byteCount = 0; - byte[] bytes = new byte[oledType.getHeight() * oledType.getWidth() / 8]; - for (int y = 0; y < oledType.getHeight();) { - for (int x = 0; x < oledType.getWidth(); x++) { - int next = 0; - int step = 0; - for (; step < 8 && y + step < oledType.getHeight(); step++) { - if (image.getRGB(x, y + step) != Color.black.getRGB()) { - next |= (1 << step); - } - } - bytes[byteCount] = (byte) next; - byteCount++; - } - y += 8; - } - return bytes; - } - - private void initialize() throws IOException { - sleep(1); - gpioResetPin.setState(DigitalState.LOW.value().intValue()); - sleep(10); - gpioResetPin.setState(DigitalState.HIGH.value().intValue()); - executeCommand(Commands.DISPLAY_OFF); - executeCommand(Commands.SET_DISPLAY_CLOCK_DIV, 0x80); - if (!useExternalVCC) { - executeCommand(Commands.CHARGE_PUMP, CHARGE_PUMP_VALUE_ENABLE); - } else { - executeCommand(Commands.CHARGE_PUMP, CHARGE_PUMP_VALUE_DISABLE); - } - setMemoryMode(MemoryModes.HORIZONTAL); - executeCommand(Commands.SEGMENT_REMAP_127); - executeCommand(Commands.SET_MULTIPLEX_RATIO, oledType.getHeight() - 1); - executeCommand(Commands.SET_DISPLAY_OFFSET, 0); - executeCommand(Commands.SET_START_LINE_ZERO); - executeCommand(Commands.COM_OUTPUT_SCAN_DIR_DESCENDING); - executeCommand(Commands.SET_COM_PINS, oledType.getComPins()); - executeCommand(Commands.SET_CONTRAST, DEFAULT_CONTRAST); - executeCommand(Commands.SET_PRE_CHARGE_PERIOD, useExternalVCC ? 0x22 : 0xf1); - executeCommand(Commands.SET_VCOM_DESELECT_LEVEL, 0x40); - executeCommand(Commands.RAM_CONTENT_DISPLAY); - executeCommand(Commands.INVERTED_OFF); - executeCommand(Commands.DEACTIVATE_SCROLL); - executeCommand(Commands.DISPLAY_ON); - } - - private void executeCommand(Commands command, int value1, int value2) throws IOException { - executeCommand(command); - writeCommand((byte) value1); - writeCommand((byte) value2); - } - - private void executeCommand(Commands command, int value) throws IOException { - executeCommand(command); - writeCommand((byte) value); - } - - private void executeCommand(Commands command) throws IOException { - writeCommand(command.getCommandValue()); - } - - private void writeCommand(byte commandValue) throws IOException { - // TODO: correct - writeByte(0x00, commandValue); - } - - private void setMemoryMode(MemoryModes mode) throws IOException { - executeCommand(Commands.MEMORY_MODE, mode.getValue()); - } + private final byte commandValue; + + Commands(byte commandValue) { + this.commandValue = commandValue; + } + + public byte getCommandValue() { + return commandValue; + } + } + + private enum MemoryModes { + HORIZONTAL((byte) 0), VERTICAL((byte) 1), PAGE((byte) 2); + + private byte value; + + MemoryModes(byte value) { + this.value = value; + } + + public byte getValue() { + return value; + } + + } + + /** + * Constructor. + * + * @param variant the oled variant, most commonly the 32 or 64 line version. + * @param resetPin the GPIO pin used for the reset. + * @throws IOException if there was a communication problem. + */ + public SSD1306Device(OLEDVariant variant, GpioPin resetPin) throws IOException { + this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, variant, resetPin, false); + } + + /** + * Constructor. + * + * @param bus the I2C bus used. + * @param address the I2C address of the OLED device, most commonly 0x3c. + * @param oledType the variant of the oled (depends on which version you own). + * @param resetPinId the GPIO pin used for reset (depends on your wiring). + * @param useExternalVCC use external VCC to drive the OLED. If false, the internal + * charge pump will be used to regulate to the necessary voltage. + * This is most commonly false. + * @throws IOException if there was a communication problem. + */ + public SSD1306Device(I2cBus bus, int address, OLEDVariant oledType, GpioPin resetPinId, boolean useExternalVCC) throws IOException { + super(bus, address); + this.image = new BufferedImage(oledType.getWidth(), oledType.getHeight(), BufferedImage.TYPE_BYTE_BINARY); + + var pi4jRpiContext = Pi4J.newAutoContext(); + var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); + var gpioResetPinConfig = digitalOutputBuilder.address(resetPinId.address()).onState(DigitalState.HIGH).build(); + + this.gpioResetPin = pi4jRpiContext.dout().create(gpioResetPinConfig); + + //this.resetPin = gpio.provisionDigitalOutputPin(resetPinId, "reset", PinState.HIGH); + this.useExternalVCC = useExternalVCC; + this.oledType = oledType; + initialize(); + } + + /** + * @return the graphics context upon which to draw. This being a monochrome + * display, only the colors {@link Color}.black and {@link Color} + * .white should be used. + */ + public Graphics2D getGraphicsContext() { + return image.createGraphics(); + } + + /** + * Pushes the image data to the device over I2C. + * + * @throws IOException exception + */ + public void pushImage() throws IOException { + executeCommand(Commands.SET_COLUMN_ADDRESS, 0, oledType.getWidth() - 1); + executeCommand(Commands.SET_PAGE_ADDRESS, 0, oledType.getPageEnd()); + + // Transmitting image data in one write + byte[] byteArray = toByteArray(); + LOGGER.debug("pushImage:{}", Arrays.toString(byteArray)); + // TODO : const + writeByteBufferByAddress(0x40, byteArray); + } + + /** + * @param enable false to disable (turn off), true to enable (turn on). + * @throws IOException exception + */ + public void setEnabled(boolean enable) throws IOException { + if (enable) { + executeCommand(Commands.DISPLAY_ON); + } else { + executeCommand(Commands.DISPLAY_OFF); + } + } + + /** + * Sets the contrast between 0 (minimum) and 1.0 (max). + * + * @param contrast a value between 0 and 1.0. + * @throws IOException exception + */ + public void setContrast(float contrast) throws IOException { + executeCommand(Commands.SET_CONTRAST, Math.max(Math.round(contrast * 0xff), 0xff)); + } + + /** + * @return the image used to draw upon. + */ + public BufferedImage getImage() { + return image; + } + + private byte[] toByteArray() { + int byteCount = 0; + byte[] bytes = new byte[oledType.getHeight() * oledType.getWidth() / 8]; + for (int y = 0; y < oledType.getHeight(); ) { + for (int x = 0; x < oledType.getWidth(); x++) { + int next = 0; + int step = 0; + for (; step < 8 && y + step < oledType.getHeight(); step++) { + if (image.getRGB(x, y + step) != Color.black.getRGB()) { + next |= (1 << step); + } + } + bytes[byteCount] = (byte) next; + byteCount++; + } + y += 8; + } + return bytes; + } + + private void initialize() throws IOException { + sleep(1); + gpioResetPin.setState(DigitalState.LOW.value().intValue()); + sleep(10); + gpioResetPin.setState(DigitalState.HIGH.value().intValue()); + executeCommand(Commands.DISPLAY_OFF); + executeCommand(Commands.SET_DISPLAY_CLOCK_DIV, 0x80); + if (!useExternalVCC) { + executeCommand(Commands.CHARGE_PUMP, CHARGE_PUMP_VALUE_ENABLE); + } else { + executeCommand(Commands.CHARGE_PUMP, CHARGE_PUMP_VALUE_DISABLE); + } + setMemoryMode(MemoryModes.HORIZONTAL); + executeCommand(Commands.SEGMENT_REMAP_127); + executeCommand(Commands.SET_MULTIPLEX_RATIO, oledType.getHeight() - 1); + executeCommand(Commands.SET_DISPLAY_OFFSET, 0); + executeCommand(Commands.SET_START_LINE_ZERO); + executeCommand(Commands.COM_OUTPUT_SCAN_DIR_DESCENDING); + executeCommand(Commands.SET_COM_PINS, oledType.getComPins()); + executeCommand(Commands.SET_CONTRAST, DEFAULT_CONTRAST); + executeCommand(Commands.SET_PRE_CHARGE_PERIOD, useExternalVCC ? 0x22 : 0xf1); + executeCommand(Commands.SET_VCOM_DESELECT_LEVEL, 0x40); + executeCommand(Commands.RAM_CONTENT_DISPLAY); + executeCommand(Commands.INVERTED_OFF); + executeCommand(Commands.DEACTIVATE_SCROLL); + executeCommand(Commands.DISPLAY_ON); + } + + private void executeCommand(Commands command, int value1, int value2) throws IOException { + executeCommand(command); + writeCommand((byte) value1); + writeCommand((byte) value2); + } + + private void executeCommand(Commands command, int value) throws IOException { + executeCommand(command); + writeCommand((byte) value); + } + + private void executeCommand(Commands command) throws IOException { + writeCommand(command.getCommandValue()); + } + + private void writeCommand(byte commandValue) throws IOException { + // TODO: correct + writeByte(0x00, commandValue); + } + + private void setMemoryMode(MemoryModes mode) throws IOException { + executeCommand(Commands.MEMORY_MODE, mode.getValue()); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Device.java index 16778dfa..bf3ff74a 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Device.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/bmp/BMP085Device.java @@ -18,6 +18,8 @@ import com.robo4j.hw.rpi.i2c.AbstractI2CDevice; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.ByteArrayInputStream; import java.io.DataInputStream; @@ -27,249 +29,232 @@ /** * Abstraction to read a Bosch digital barometric pressure sensor * (BMP085/BMP180). - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public final class BMP085Device extends AbstractI2CDevice { - private static final int DEFAULT_I2C_ADDRESS = 0x77; - private static final int PRESSURE_SEA_LEVEL = 101325; - private static final double POW_FACT = 1.0 / 5.225; - // Calibration data - private static final int CALIBRATION_START = 0xAA; - private static final int CALIBRATION_END = 0xBF; + private static final Logger LOGGER = LoggerFactory.getLogger(BMP085Device.class); + private static final int DEFAULT_I2C_ADDRESS = 0x77; + private static final int PRESSURE_SEA_LEVEL = 101325; + private static final double POW_FACT = 1.0 / 5.225; + // Calibration data + private static final int CALIBRATION_START = 0xAA; + private static final int CALIBRATION_END = 0xBF; + + private static final short BMP085_CONTROL = 0xF4; + private static final short BMP085_TEMPDATA = 0xF6; + private static final short BMP085_PRESSUREDATA = 0xF6; + private static final byte BMP085_READTEMPCMD = 0x2E; + private static final byte BMP085_READPRESSURECMD = 0x34; - private static final short BMP085_CONTROL = 0xF4; - private static final short BMP085_TEMPDATA = 0xF6; - private static final short BMP085_PRESSUREDATA = 0xF6; - private static final byte BMP085_READTEMPCMD = 0x2E; - private static final byte BMP085_READPRESSURECMD = 0x34; + private final OperatingMode mode; - private final OperatingMode mode; + // Calibration variables + private short AC1; + private short AC2; + private short AC3; + private int AC4; + private int AC5; + private int AC6; + private short B1; + private short B2; + private short MC; + private short MD; - // Calibration variables - private short AC1; - private short AC2; - private short AC3; - private int AC4; - private int AC5; - private int AC6; - private short B1; - private short B2; - private short MC; - private short MD; + // TODO review enum placement - // TODO review enum placement - /** - * Available operating modes for the BMP085. - */ - public enum OperatingMode { - /** - * Max conversion time (pressure): 4.5ms Current draw: 3µA - */ - ULTRA_LOW_POWER(45, 3), - /** - * Max conversion time (pressure): 7.5ms Current draw: 5µA - */ - STANDARD(75, 5), - /** - * Max conversion time (pressure): 13.5ms Current draw: 7µA - */ - HIGH_RES(135, 7), - /** - * Max conversion time (pressure): 25.5ms Current draw: 12µA - */ - ULTRA_HIGH_RES(255, 12); + /** + * Available operating modes for the BMP085. + */ + public enum OperatingMode { + /** + * Max conversion time (pressure): 4.5ms Current draw: 3µA + */ + ULTRA_LOW_POWER(45, 3), + /** + * Max conversion time (pressure): 7.5ms Current draw: 5µA + */ + STANDARD(75, 5), + /** + * Max conversion time (pressure): 13.5ms Current draw: 7µA + */ + HIGH_RES(135, 7), + /** + * Max conversion time (pressure): 25.5ms Current draw: 12µA + */ + ULTRA_HIGH_RES(255, 12); - private final int waitTime; - private final int currentDraw; + private final int waitTime; + private final int currentDraw; - OperatingMode(int maxConversionTime, int currentDraw) { - this.waitTime = (maxConversionTime + 5) / 10; - this.currentDraw = currentDraw; - } + OperatingMode(int maxConversionTime, int currentDraw) { + this.waitTime = (maxConversionTime + 5) / 10; + this.currentDraw = currentDraw; + } - /** - * @return the over sampling setting. - */ - public int getOverSamplingSetting() { - return this.ordinal(); - } + /** + * @return the over sampling setting. + */ + public int getOverSamplingSetting() { + return this.ordinal(); + } - /** - * @return time to wait for a result, in ms. - */ - public int getWaitTime() { - return waitTime; - } + /** + * @return time to wait for a result, in ms. + */ + public int getWaitTime() { + return waitTime; + } - /** - * @return the average typical current at 1 sample per second, in µA. - */ - public int getCurrentDraw() { - return currentDraw; - } - } + /** + * @return the average typical current at 1 sample per second, in µA. + */ + public int getCurrentDraw() { + return currentDraw; + } + } - /** - * Constructs a BMPDevice using the default settings. (I2CBUS.BUS_1, 0x77) - * - * @see BMP085Device - * - * @param mode - * operating mode - * - * @throws IOException - * if there was communication problem - */ - public BMP085Device(OperatingMode mode) throws IOException { - // 0x77 is the default address used by the AdaFruit BMP board. - this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, mode); - } + /** + * Constructs a BMPDevice using the default settings. (I2CBUS.BUS_1, 0x77) + * + * @param mode operating mode + * @throws IOException if there was communication problem + * @see BMP085Device + */ + public BMP085Device(OperatingMode mode) throws IOException { + // 0x77 is the default address used by the AdaFruit BMP board. + this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS, mode); + } - /** - * Creates a software interface to an Adafruit BMP board (BMP085). - * - * @param bus - * the I2C bus to use. - * @param address - * the address to use. - * @param mode - * operating mode - * - * @see I2cBus documentation - * - * @throws IOException - * if there was communication problem - */ - public BMP085Device(I2cBus bus, int address, OperatingMode mode) throws IOException { - super(bus, address); - this.mode = mode; - readCalibrationData(); - } + /** + * Creates a software interface to an Adafruit BMP board (BMP085). + * + * @param bus the I2C bus to use. + * @param address the address to use. + * @param mode operating mode + * @throws IOException if there was communication problem + * @see I2cBus documentation + */ + public BMP085Device(I2cBus bus, int address, OperatingMode mode) throws IOException { + super(bus, address); + this.mode = mode; + readCalibrationData(); + } - /** - * Returns the temperature in degrees Celcius. - * - * @return the temperature in degrees Celcius. - * @throws IOException - * if there was communication problem - */ - public float readTemperature() throws IOException { - int UT = readRawTemp(); - int X1 = ((UT - AC6) * AC5) >> 15; - int X2 = (MC << 11) / (X1 + MD); - int B5 = X1 + X2; - return ((B5 + 8) >> 4) / 10.0f; - } + /** + * Returns the temperature in degrees Celcius. + * + * @return the temperature in degrees Celcius. + * @throws IOException if there was communication problem + */ + public float readTemperature() throws IOException { + int UT = readRawTemp(); + int X1 = ((UT - AC6) * AC5) >> 15; + int X2 = (MC << 11) / (X1 + MD); + int B5 = X1 + X2; + return ((B5 + 8) >> 4) / 10.0f; + } - /** - * Returns the pressure in Pascal. - * - * @return the pressure in Pascal. - * @throws IOException - * if there was communication problem - */ - public int readPressure() throws IOException { - long p = 0; - int UT = readRawTemp(); - int UP = readRawPressure(); + /** + * Returns the pressure in Pascal. + * + * @return the pressure in Pascal. + * @throws IOException if there was communication problem + */ + public int readPressure() throws IOException { + long p = 0; + int UT = readRawTemp(); + int UP = readRawPressure(); - int X1 = ((UT - AC6) * AC5) >> 15; - int X2 = (MC << 11) / (X1 + MD); - int B5 = X1 + X2; + int X1 = ((UT - AC6) * AC5) >> 15; + int X2 = (MC << 11) / (X1 + MD); + int B5 = X1 + X2; - int B6 = B5 - 4000; - X1 = (B2 * ((B6 * B6) >> 12)) >> 11; - X2 = (AC2 * B6) >> 11; - int X3 = X1 + X2; - int B3 = (((AC1 * 4 + X3) << mode.getOverSamplingSetting()) + 2) / 4; + int B6 = B5 - 4000; + X1 = (B2 * ((B6 * B6) >> 12)) >> 11; + X2 = (AC2 * B6) >> 11; + int X3 = X1 + X2; + int B3 = (((AC1 * 4 + X3) << mode.getOverSamplingSetting()) + 2) / 4; - X1 = (AC3 * B6) >> 13; - X2 = (B1 * ((B6 * B6) >> 12)) >> 16; - X3 = ((X1 + X2) + 2) >> 2; - long B4 = (AC4 * ((long) (X3 + 32768))) >> 15; - long B7 = ((long) UP - B3) * (50000 >> mode.getOverSamplingSetting()); + X1 = (AC3 * B6) >> 13; + X2 = (B1 * ((B6 * B6) >> 12)) >> 16; + X3 = ((X1 + X2) + 2) >> 2; + long B4 = (AC4 * ((long) (X3 + 32768))) >> 15; + long B7 = ((long) UP - B3) * (50000 >> mode.getOverSamplingSetting()); - if (B7 < 0x80000000) { - p = (B7 * 2) / B4; - } else { - p = (B7 / B4) * 2; - } + if (B7 < 0x80000000) { + p = (B7 * 2) / B4; + } else { + p = (B7 / B4) * 2; + } - X1 = (int) ((p >> 8) * (p >> 8)); - X1 = (X1 * 3038) >> 16; - X2 = (int) (-7357 * p) >> 16; - p = p + ((X1 + X2 + 3791) >> 4); - return (int) p; - } + X1 = (int) ((p >> 8) * (p >> 8)); + X1 = (X1 * 3038) >> 16; + X2 = (int) (-7357 * p) >> 16; + p = p + ((X1 + X2 + 3791) >> 4); + return (int) p; + } - /** - * Returns the barometric altitude above sea level in meters. - * - * @return the barometric altitude above sea level in meters. - * @throws IOException - * if there was communication problem - */ - public float readAltitude() throws IOException { - float pressure = readPressure(); - return (float) (44330.0 * (1.0 - Math.pow(pressure / PRESSURE_SEA_LEVEL, POW_FACT))); - } + /** + * Returns the barometric altitude above sea level in meters. + * + * @return the barometric altitude above sea level in meters. + * @throws IOException if there was communication problem + */ + public float readAltitude() throws IOException { + float pressure = readPressure(); + return (float) (44330.0 * (1.0 - Math.pow(pressure / PRESSURE_SEA_LEVEL, POW_FACT))); + } - /** - * Returns the raw temperature sensor data. Mostly for debugging. - * - * @return the raw temperature sensor data. - * @throws IOException - * if there was a communication problem - */ - public int readRawTemp() throws IOException { + /** + * Returns the raw temperature sensor data. Mostly for debugging. + * + * @return the raw temperature sensor data. + * @throws IOException if there was a communication problem + */ + public int readRawTemp() throws IOException { // i2CConfig.write(BMP085_CONTROL, BMP085_READTEMPCMD); - writeByte(BMP085_CONTROL, BMP085_READTEMPCMD); - sleep(50); - return readU2(BMP085_TEMPDATA); - } + writeByte(BMP085_CONTROL, BMP085_READTEMPCMD); + sleep(50); + return readU2(BMP085_TEMPDATA); + } - /** - * Returns the raw pressure sensor data. Mostly for debugging. - * - * @return the raw pressure sensor data. - * @throws IOException - * if there was a communication problem - */ - public int readRawPressure() throws IOException { + /** + * Returns the raw pressure sensor data. Mostly for debugging. + * + * @return the raw pressure sensor data. + * @throws IOException if there was a communication problem + */ + public int readRawPressure() throws IOException { // i2CConfig.write(BMP085_CONTROL, BMP085_READPRESSURECMD); - writeByte(BMP085_CONTROL, BMP085_READPRESSURECMD); - sleep(mode.getWaitTime()); - return readU3(BMP085_PRESSUREDATA) >> (8 - mode.getOverSamplingSetting()); - } + writeByte(BMP085_CONTROL, BMP085_READPRESSURECMD); + sleep(mode.getWaitTime()); + return readU3(BMP085_PRESSUREDATA) >> (8 - mode.getOverSamplingSetting()); + } - private void readCalibrationData() throws IOException { - int totalBytes = CALIBRATION_END - CALIBRATION_START + 1; - byte[] bytes = new byte[totalBytes]; + private void readCalibrationData() throws IOException { + int totalBytes = CALIBRATION_END - CALIBRATION_START + 1; + byte[] bytes = new byte[totalBytes]; // int bytesRead = i2CConfig.read(CALIBRATION_START, bytes, 0, totalBytes); - int bytesRead = readBufferByAddress(CALIBRATION_START, bytes, 0, totalBytes); - if (bytesRead != totalBytes) { - throw new IOException("Could not read calibration data. Read " + Arrays.toString(bytes) + " of " + totalBytes); - } + int bytesRead = readBufferByAddress(CALIBRATION_START, bytes, 0, totalBytes); + if (bytesRead != totalBytes) { + throw new IOException("Could not read calibration data. Read " + Arrays.toString(bytes) + " of " + totalBytes); + } - DataInputStream calibrationData = new DataInputStream(new ByteArrayInputStream(bytes)); - AC1 = calibrationData.readShort(); - AC2 = calibrationData.readShort(); - AC3 = calibrationData.readShort(); - AC4 = calibrationData.readUnsignedShort(); - AC5 = calibrationData.readUnsignedShort(); - AC6 = calibrationData.readUnsignedShort(); - B1 = calibrationData.readShort(); - B2 = calibrationData.readShort(); - calibrationData.readShort(); // MB not used for anything it seems... - MC = calibrationData.readShort(); - MD = calibrationData.readShort(); + DataInputStream calibrationData = new DataInputStream(new ByteArrayInputStream(bytes)); + AC1 = calibrationData.readShort(); + AC2 = calibrationData.readShort(); + AC3 = calibrationData.readShort(); + AC4 = calibrationData.readUnsignedShort(); + AC5 = calibrationData.readUnsignedShort(); + AC6 = calibrationData.readUnsignedShort(); + B1 = calibrationData.readShort(); + B2 = calibrationData.readShort(); + calibrationData.readShort(); // MB not used for anything it seems... + MC = calibrationData.readShort(); + MD = calibrationData.readShort(); - if (Boolean.getBoolean("se.hirt.pi.adafruit.debug")) { - System.out - .println(String.format("AC1:%d, AC2:%d, AC3:%d, AC4:%d, AC5:%d, AC6:%d, B1:%d, B2:%d, MC:%d, MD:%d", - AC1, AC2, AC3, AC4, AC5, AC6, B1, B2, MC, MD)); - } - } + LOGGER.debug("AC1:{}, AC2:{}, AC3:{}, AC4:{}, AC5:{}, AC6:{}, B1:{}, B2:{}, MC:{}, MD:{}", AC1, AC2, AC3, AC4, AC5, AC6, B1, B2, MC, MD); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java index 77617c90..552dfda5 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java @@ -16,23 +16,15 @@ */ package com.robo4j.hw.rpi.i2c.gps; -import com.robo4j.hw.rpi.gps.GPS; -import com.robo4j.hw.rpi.gps.GPSListener; -import com.robo4j.hw.rpi.gps.NmeaUtils; -import com.robo4j.hw.rpi.gps.PositionEvent; -import com.robo4j.hw.rpi.gps.VelocityEvent; +import com.robo4j.hw.rpi.gps.*; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.List; import java.util.StringTokenizer; -import java.util.concurrent.CopyOnWriteArrayList; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; -import java.util.logging.Level; -import java.util.logging.Logger; +import java.util.concurrent.*; /** * The GPS class for the SparkFun GPS. @@ -41,122 +33,124 @@ * @author Miro Wengner (@miragemiko) */ public class TitanX1GPS implements GPS { - private static final long READ_INTERVAL = 1000; - - private final XA1110Device device; - private final List listeners = new CopyOnWriteArrayList(); - private final GPSDataRetriever retriever = new GPSDataRetriever(); - - private final ScheduledExecutorService internalExecutor = Executors.newScheduledThreadPool(1, (r) -> { - Thread t = new Thread(r, "GPS Internal Executor"); - t.setDaemon(true); - return t; - }); - - // This is the scheduled future controlling the auto updates. - private ScheduledFuture scheduledFuture; - - public TitanX1GPS() throws IOException { - this.device = new XA1110Device(); - } - - public TitanX1GPS(I2cBus bus, int address) throws IOException { - this.device = new XA1110Device(bus, address); - } - - @Override - public void addListener(GPSListener gpsListener) { - listeners.add(gpsListener); - } - - @Override - public void removeListener(GPSListener gpsListener) { - listeners.remove(gpsListener); - } - - @Override - public void start() { - synchronized (internalExecutor) { - if (scheduledFuture == null) { - scheduledFuture = internalExecutor.scheduleAtFixedRate(retriever, 0, READ_INTERVAL, TimeUnit.MILLISECONDS); - } else { - throw new IllegalStateException("Already running!"); - } - } - } - - @Override - public void shutdown() { - synchronized (internalExecutor) { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - awaitTermination(); - } - } - - private final class GPSDataRetriever implements Runnable { - private final StringBuilder builder = new StringBuilder(); - - @Override - public void run() { - update(); - } - - public void update() { - String str = ""; - try { - str = readNext(builder); - } catch (IllegalStateException | IOException e) { - Logger.getLogger(TitanX1GPS.class.getName()).log(Level.WARNING, "Error reading line", e); - } - builder.setLength(0); - StringTokenizer st = new StringTokenizer(str, "\n", true); - while (st.hasMoreElements()) { - String dataLine = st.nextToken().trim(); - if (!dataLine.isEmpty()) { - String line = NmeaUtils.cleanLine(dataLine); - consume(line); - } - } - } - - private void consume(String dataLine) { - if (dataLine.startsWith("$")) { - if (NmeaUtils.hasValidCheckSum(dataLine)) { - if (XA1110PositionEvent.isAcceptedLine(dataLine)) { - notifyPosition(XA1110PositionEvent.decode(TitanX1GPS.this, dataLine)); - } else if (XA1110VelocityEvent.isAcceptedLine(dataLine)) { - notifyVelocity(XA1110VelocityEvent.decode(TitanX1GPS.this, dataLine)); - } - } - } - } - - private String readNext(StringBuilder builder) throws IllegalStateException, IOException { - device.readGpsData(builder); - return builder.toString(); - } - } - - private void notifyPosition(PositionEvent event) { - for (GPSListener listener : listeners) { - listener.onPosition(event); - } - } - - private void notifyVelocity(VelocityEvent event) { - for (GPSListener listener : listeners) { - listener.onVelocity(event); - } - } - - private void awaitTermination() { - try { - internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS); - internalExecutor.shutdown(); - } catch (InterruptedException e) { - // Don't care if we were interrupted. - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(TitanX1GPS.class); + private static final long READ_INTERVAL = 1000; + + private final XA1110Device device; + private final List listeners = new CopyOnWriteArrayList(); + private final GPSDataRetriever retriever = new GPSDataRetriever(); + + private final ScheduledExecutorService internalExecutor = Executors.newScheduledThreadPool(1, (r) -> { + Thread t = new Thread(r, "GPS Internal Executor"); + t.setDaemon(true); + return t; + }); + + // This is the scheduled future controlling the auto updates. + private ScheduledFuture scheduledFuture; + + public TitanX1GPS() throws IOException { + this.device = new XA1110Device(); + } + + public TitanX1GPS(I2cBus bus, int address) throws IOException { + this.device = new XA1110Device(bus, address); + } + + @Override + public void addListener(GPSListener gpsListener) { + listeners.add(gpsListener); + } + + @Override + public void removeListener(GPSListener gpsListener) { + listeners.remove(gpsListener); + } + + @Override + public void start() { + synchronized (internalExecutor) { + if (scheduledFuture == null) { + scheduledFuture = internalExecutor.scheduleAtFixedRate(retriever, 0, READ_INTERVAL, TimeUnit.MILLISECONDS); + } else { + throw new IllegalStateException("Already running!"); + } + } + } + + @Override + public void shutdown() { + synchronized (internalExecutor) { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + awaitTermination(); + } + } + + private final class GPSDataRetriever implements Runnable { + private final StringBuilder builder = new StringBuilder(); + + @Override + public void run() { + update(); + } + + public void update() { + String str = ""; + try { + str = readNext(builder); + } catch (IllegalStateException | IOException e) { + LOGGER.error("Error reading line", e); + } + builder.setLength(0); + StringTokenizer st = new StringTokenizer(str, "\n", true); + while (st.hasMoreElements()) { + String dataLine = st.nextToken().trim(); + if (!dataLine.isEmpty()) { + String line = NmeaUtils.cleanLine(dataLine); + consume(line); + } + } + } + + private void consume(String dataLine) { + if (dataLine.startsWith("$")) { + if (NmeaUtils.hasValidCheckSum(dataLine)) { + if (XA1110PositionEvent.isAcceptedLine(dataLine)) { + notifyPosition(XA1110PositionEvent.decode(TitanX1GPS.this, dataLine)); + } else if (XA1110VelocityEvent.isAcceptedLine(dataLine)) { + notifyVelocity(XA1110VelocityEvent.decode(TitanX1GPS.this, dataLine)); + } + } + } + } + + private String readNext(StringBuilder builder) throws IllegalStateException, IOException { + device.readGpsData(builder); + return builder.toString(); + } + } + + private void notifyPosition(PositionEvent event) { + for (GPSListener listener : listeners) { + listener.onPosition(event); + } + } + + private void notifyVelocity(VelocityEvent event) { + for (GPSListener listener : listeners) { + listener.onVelocity(event); + } + } + + private void awaitTermination() { + try { + internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS); + internalExecutor.shutdown(); + } catch (InterruptedException e) { + LOGGER.error("awaitTermination", e); + // Don't care if we were interrupted. + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110Device.java index b9970c91..e5e7c3d7 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110Device.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110Device.java @@ -18,6 +18,8 @@ import com.robo4j.hw.rpi.i2c.AbstractI2CDevice; import com.robo4j.hw.rpi.utils.I2cBus; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.nio.charset.Charset; @@ -35,6 +37,8 @@ * @author Miro Wengner (@miragemiko) */ class XA1110Device extends AbstractI2CDevice { + private static final Logger LOGGER = LoggerFactory.getLogger(XA1110Device.class); + public enum NmeaSentenceType { //@formatter:off GEOPOS("GLL", "Geographic Position - Latitude longitude", 0), @@ -284,7 +288,7 @@ void readGpsData(StringBuilder builder) throws IOException { } private void init() throws IOException { - System.out.println("Initializing device"); + LOGGER.info("Initializing device"); sendMtkPacket(createNmeaSentencesAndFrequenciesMtkPacket(new NmeaSetting(NmeaSentenceType.FIX_DATA, 1), new NmeaSetting(NmeaSentenceType.COURSE_AND_SPEED, 1))); @@ -296,22 +300,21 @@ public static void main(String[] args) throws IOException, InterruptedException while (true) { StringBuilder builder = new StringBuilder(); try { - System.out.println("=== Reading GPS-data from i2c bus to builder ==="); + LOGGER.info("=== Reading GPS-data from i2c bus to builder ==="); device.readGpsData(builder); String gpsData = builder.toString(); - System.out.println("Got data of length " + gpsData.length()); + LOGGER.info("Got data of length {}", gpsData.length()); gpsData = gpsData.replace("$", "\n$"); - System.out.print(gpsData); - System.out.println("\n======"); + LOGGER.info("gpsData:{}", gpsData); + LOGGER.info("======"); TimeUnit.MILLISECONDS.sleep(1000); } catch (Exception e) { - // TODO improve - e.printStackTrace(); + LOGGER.error(e.getMessage(), e); } } }); t.setDaemon(true); - System.out.println("Starting reading from GPS. Press to quit!"); + LOGGER.info("Starting reading from GPS. Press to quit!"); t.start(); System.in.read(); } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java index 6010b815..20440cc8 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java @@ -19,68 +19,71 @@ import com.robo4j.hw.rpi.i2c.CalibratedFloat3DDevice; import com.robo4j.hw.rpi.i2c.ReadableDevice; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.Arrays; public class CalibratedGyro extends CalibratedFloat3DDevice { - private static final int NUMBER_OF_CALIBRATION_READINGS = 40; - private static final int NUMBER_OF_CALIBRATION_READINGS_TO_DROP = 5; - private static final Tuple3f RANGE_MULTIPLIERS = new Tuple3f(1, 1, -1); + private static final Logger LOGGER = LoggerFactory.getLogger(CalibratedGyro.class); + private static final int NUMBER_OF_CALIBRATION_READINGS = 40; + private static final int NUMBER_OF_CALIBRATION_READINGS_TO_DROP = 5; + private static final Tuple3f RANGE_MULTIPLIERS = new Tuple3f(1, 1, -1); - public CalibratedGyro(ReadableDevice device) { - super(device, new Tuple3f(), RANGE_MULTIPLIERS.copy()); - } + public CalibratedGyro(ReadableDevice device) { + super(device, new Tuple3f(), RANGE_MULTIPLIERS.copy()); + } - public CalibratedGyro(ReadableDevice device, Tuple3f offsets, Tuple3f multipliers) { - super(device, offsets, multipliers); - } + public CalibratedGyro(ReadableDevice device, Tuple3f offsets, Tuple3f multipliers) { + super(device, offsets, multipliers); + } - /** - * Does calibration. - * - * @throws IOException - * exception - */ - public void calibrate() throws IOException { - setCalibration(new Tuple3f(), Tuple3f.createIdentity()); - float[] xvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; - float[] yvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; - float[] zvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; + /** + * Does calibration. + * + * @throws IOException exception + */ + public void calibrate() throws IOException { + setCalibration(new Tuple3f(), Tuple3f.createIdentity()); + float[] xvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; + float[] yvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; + float[] zvalues = new float[NUMBER_OF_CALIBRATION_READINGS]; - for (int i = 0; i < NUMBER_OF_CALIBRATION_READINGS; i++) { - Tuple3f tmp = read(); - xvalues[i] = tmp.x; - yvalues[i] = tmp.y; - zvalues[i] = tmp.z; - sleep(20); - } - Tuple3f calibration = new Tuple3f(); - calibration.x = calibrate(xvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); - calibration.y = calibrate(yvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); - calibration.z = calibrate(zvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); - System.out.println(RANGE_MULTIPLIERS); - setCalibration(calibration, RANGE_MULTIPLIERS); - } + for (int i = 0; i < NUMBER_OF_CALIBRATION_READINGS; i++) { + Tuple3f tmp = read(); + xvalues[i] = tmp.x; + yvalues[i] = tmp.y; + zvalues[i] = tmp.z; + sleep(20); + } + Tuple3f calibration = new Tuple3f(); + calibration.x = calibrate(xvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); + calibration.y = calibrate(yvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); + calibration.z = calibrate(zvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP); + LOGGER.info("calibrate:{}", RANGE_MULTIPLIERS); + setCalibration(calibration, RANGE_MULTIPLIERS); + } - /** - * Simple calibration function. Drop the n highest and lowest, next take the - * average of what is left. - */ - private float calibrate(float[] values, int drop) { - Arrays.sort(values); - double calibrationSum = 0; - for (int i = drop; i < values.length - drop; i++) { - calibrationSum += values[i]; - } - return (float) -calibrationSum / (values.length - 2 * drop); - } + /** + * Simple calibration function. Drop the n highest and lowest, next take the + * average of what is left. + */ + private float calibrate(float[] values, int drop) { + Arrays.sort(values); + double calibrationSum = 0; + for (int i = drop; i < values.length - drop; i++) { + calibrationSum += values[i]; + } + return (float) -calibrationSum / (values.length - 2 * drop); + } - private void sleep(int millis) { - try { - Thread.sleep(millis); - } catch (InterruptedException e) { - // Don't care - } - } + // TODO : review usage + private void sleep(int millis) { + try { + Thread.sleep(millis); + } catch (InterruptedException e) { + LOGGER.error(e.getMessage(), e); + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Device.java index 2ae08cd6..18e4d63e 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Device.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/magnetometer/MagnetometerLSM303Device.java @@ -22,6 +22,8 @@ import com.robo4j.math.geometry.Matrix3f; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.math.geometry.Tuple3i; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; @@ -34,6 +36,7 @@ */ // FIXME(Marcus/Dec 5, 2016): Verify that this one works. public class MagnetometerLSM303Device extends AbstractI2CDevice implements ReadableDevice { + private static final Logger LOGGER = LoggerFactory.getLogger(MagnetometerLSM303Device.class); private static final int DEFAULT_I2C_ADDRESS = 0x1e; private static final int CRA_REG_M = 0x00; private static final int CRB_REG_M = 0x01; @@ -167,7 +170,7 @@ public synchronized Tuple3i readRaw() throws IOException { // int n = i2CConfig.read(OUT_X_H_M, data, 0, RESULT_BUFFER_SIZE); int n = readBufferByAddress(OUT_X_H_M, data, 0, RESULT_BUFFER_SIZE); if (n != RESULT_BUFFER_SIZE) { - getLogger().warning("Failed to read all data from accelerometer. Should have read 6, could only read " + n); + LOGGER.warn("Failed to read all data from accelerometer. Should have read 6, could only read {}", n); } // Yep, this is indeed the correct order ;) rawData.x = read16bitSigned(data, 0); diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java index d41e3284..5024161b 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java @@ -17,16 +17,6 @@ package com.robo4j.hw.rpi.imu.bno.impl; -import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.EMPTY_EVENT; -import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.calculateNumberOfBytesInPacket; -import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.intToFloat; -import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.toInt8U; - -import java.io.IOException; -import java.util.concurrent.CountDownLatch; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicInteger; - import com.pi4j.Pi4J; import com.pi4j.context.Context; import com.pi4j.io.gpio.digital.DigitalOutput; @@ -38,18 +28,18 @@ import com.robo4j.hw.rpi.imu.bno.DataEventType; import com.robo4j.hw.rpi.imu.bno.DataListener; import com.robo4j.hw.rpi.imu.bno.VectorEvent; -import com.robo4j.hw.rpi.imu.bno.shtp.ControlReportId; -import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpChannel; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperation; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperationBuilder; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperationResponse; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketRequest; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketResponse; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpReportIds; -import com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils; +import com.robo4j.hw.rpi.imu.bno.shtp.*; import com.robo4j.hw.rpi.utils.GpioPin; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; + +import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.*; /** * Abstraction for a BNO080 absolute orientation device. @@ -63,588 +53,555 @@ * sensors)
* Channel 5: gyro rotation vector
*

- * + *

* https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/blob/master/src/SparkFun_BNO080_Arduino_Library.cpp - * + *

* RPi/Pi4j pins https://pi4j.com/1.2/pins/model-3b-rev1.html * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Bno080SPIDevice extends AbstractBno080Device { - public static final SpiMode DEFAULT_SPI_MODE = SpiMode.MODE_3; - // 3MHz maximum SPI speed - public static final int DEFAULT_SPI_SPEED = 3000000; - public static final SpiChipSelect DEFAULT_SPI_CHANNEL = SpiChipSelect.CS_0; - - public static final int MAX_PACKET_SIZE = 32762; - public static final int DEFAULT_TIMEOUT_MS = 1000; - public static final int UNIT_TICK_MICRO = 100; - public static final int TIMEBASE_REFER_DELTA = 120; - public static final int MAX_SPI_COUNT = 255; - public static final int MAX_SPI_WAIT_CYCLES = 2; - private static final int MAX_COUNTER = 255; - private final AtomicInteger spiWaitCounter = new AtomicInteger(); - - private final Spi spiDevice; - // private GpioPinDigitalInput intGpio; - // private GpioPinDigitalOutput wakeGpio; - // private GpioPinDigitalOutput rstGpio; - // private GpioPinDigitalOutput csGpio; // select slave SS = chip select CS - - private DigitalOutput intGpio; - private DigitalOutput wakeGpio; - private DigitalOutput rstGpio; - private DigitalOutput csGpio; // select slave SS = chip select CS - - // uint32_t - private long sensorReportDelayMicroSec = 0; - - /** - * Constructor. - * - * @throws IOException - * @throws InterruptedException - */ - public Bno080SPIDevice() throws IOException, InterruptedException { - this(DEFAULT_SPI_CHANNEL, DEFAULT_SPI_MODE, DEFAULT_SPI_SPEED); - } - - /** - * Constructor. - * - * @param channel - * the {@link SpiChipSelect} to use. - * @param mode - * the {@link SpiMode} to use. - * @param speed - * the speed in Hz to use for communication. - * @throws IOException - * @throws InterruptedException - */ - public Bno080SPIDevice(SpiChipSelect channel, SpiMode mode, int speed) throws IOException, InterruptedException { - // this(channel, mode, speed, RaspiPin.GPIO_00, RaspiPin.GPIO_25, - // RaspiPin.GPIO_02, RaspiPin.GPIO_03); - this(channel, mode, speed, GpioPin.GPIO_00, GpioPin.GPIO_25, GpioPin.GPIO_02, GpioPin.GPIO_03); - } - - /** - * Constructor. - * - * @param channel - * the {@link SpiChipSelect} to use. - * @param mode - * the {@link SpiMode} to use. - * @param speed - * the speed in Hz to use for communication. - * @param wake - * Used to wake the processor from a sleep mode. Active low. - * @param cs - * Chip select, active low, used as chip select/slave select on SPI - * @param reset - * Reset signal, active low, pull low to reset IC - * @param interrupt - * Interrupt, active low, pulls low when the BNO080 is ready for - * communication. - **/ - public Bno080SPIDevice(SpiChipSelect channel, SpiMode mode, int speed, GpioPin wake, GpioPin cs, GpioPin reset, - GpioPin interrupt) throws IOException, InterruptedException { - var pi4jRpiContext = Pi4J.newAutoContext(); - var spiConfig = Spi.newConfigBuilder(pi4jRpiContext).id("rpi-spi").chipSelect(channel).baud(speed).mode(mode) - .build(); - // spiDevice = new SpiDeviceImpl(channel, speed, mode); - spiDevice = pi4jRpiContext.spi().create(spiConfig); - configureSpiPins(pi4jRpiContext, wake, cs, reset, interrupt); - } - - public boolean isActive() { - return active.get(); - } - - @Override - public boolean start(SensorReportId report, int reportPeriod) { - if (reportPeriod > 0) { - final CountDownLatch latch = new CountDownLatch(1); - System.out.printf("START: ready:%s, active:%s%n", ready.get(), active.get()); - spiWaitCounter.set(0); - synchronized (executor) { - if (!ready.get()) { - initAndActive(latch, report, reportPeriod); - } else { - reactivate(latch, report, reportPeriod); - } - if (waitForLatch(latch)) { - executor.execute(() -> { - active.set(ready.get()); - executeListenerJob(); - }); - } - } - } else { - System.out.printf("start: not valid sensor:%s delay: %d%n", report, reportPeriod); - } - return active.get(); - - } - - /** - * stop forces a soft reset command unit needs 700 millis to become to be - * available - * - * @return status - */ - @Override - public boolean stop() { - CountDownLatch latch = new CountDownLatch(1); - if (ready.get() && active.get()) { - active.set(false); - if (softReset()) { - try { - TimeUnit.MILLISECONDS.sleep(700); - } catch (InterruptedException e) { - throw new IllegalStateException("not possible stop"); - } - latch.countDown(); - return true; - } else { - System.out.println("SOFT FALSE"); - } - } - return false; - } - - @Override - public void calibrate(long timeout) { - ShtpPacketRequest createCalibrateCommandAll = createCalibrateCommandAll(); - final CountDownLatch latch = new CountDownLatch(1); - executor.submit(() -> { - // TODO - track initialization better - try { - sendPacket(createCalibrateCommandAll); - } catch (InterruptedException | IOException e) { - System.out.println("Calibration failed!"); - // TODO : correct exceptions - e.printStackTrace(); - } - // TODO - wait for calibration to be good enough... - // TODO - stop calibration - latch.countDown(); - }); - } - - /** - * Calibration command. - */ - private ShtpPacketRequest createCalibrateCommandAll() { - ShtpChannel shtpChannel = ShtpChannel.COMMAND; - ShtpPacketRequest packet = prepareShtpPacketRequest(shtpChannel, 12); - packet.addBody(0, ControlReportId.COMMAND_REQUEST.getId()); - packet.addBody(0, commandSequenceNumber.getAndIncrement()); - packet.addBody(2, CommandId.ME_CALIBRATE.getId()); - packet.addBody(3, 1); - packet.addBody(4, 1); - packet.addBody(5, 1); - return packet; - } - - // private ShtpPacketRequest createSensorReportRequest(ControlReportId type, - // SensorReportId sensor) - // throws InterruptedException, IOException { - // ShtpChannel shtpChannel = ShtpChannel.CONTROL; - // ShtpPacketRequest packetRequest = prepareShtpPacketRequest(shtpChannel, - // 2); - // + private static final Logger LOGGER = LoggerFactory.getLogger(Bno080SPIDevice.class); + public static final SpiMode DEFAULT_SPI_MODE = SpiMode.MODE_3; + // 3MHz maximum SPI speed + public static final int DEFAULT_SPI_SPEED = 3000000; + public static final SpiChipSelect DEFAULT_SPI_CHANNEL = SpiChipSelect.CS_0; + + public static final int MAX_PACKET_SIZE = 32762; + public static final int DEFAULT_TIMEOUT_MS = 1000; + public static final int UNIT_TICK_MICRO = 100; + public static final int TIMEBASE_REFER_DELTA = 120; + public static final int MAX_SPI_COUNT = 255; + public static final int MAX_SPI_WAIT_CYCLES = 2; + private static final int MAX_COUNTER = 255; + private final AtomicInteger spiWaitCounter = new AtomicInteger(); + + private final Spi spiDevice; + // private GpioPinDigitalInput intGpio; + // private GpioPinDigitalOutput wakeGpio; + // private GpioPinDigitalOutput rstGpio; + // private GpioPinDigitalOutput csGpio; // select slave SS = chip select CS + + private DigitalOutput intGpio; + private DigitalOutput wakeGpio; + private DigitalOutput rstGpio; + private DigitalOutput csGpio; // select slave SS = chip select CS + + // uint32_t + private long sensorReportDelayMicroSec = 0; + + /** + * Constructor. + * + * @throws IOException + * @throws InterruptedException + */ + public Bno080SPIDevice() throws IOException, InterruptedException { + this(DEFAULT_SPI_CHANNEL, DEFAULT_SPI_MODE, DEFAULT_SPI_SPEED); + } + + /** + * Constructor. + * + * @param channel the {@link SpiChipSelect} to use. + * @param mode the {@link SpiMode} to use. + * @param speed the speed in Hz to use for communication. + * @throws IOException + * @throws InterruptedException + */ + public Bno080SPIDevice(SpiChipSelect channel, SpiMode mode, int speed) throws IOException, InterruptedException { + // this(channel, mode, speed, RaspiPin.GPIO_00, RaspiPin.GPIO_25, + // RaspiPin.GPIO_02, RaspiPin.GPIO_03); + this(channel, mode, speed, GpioPin.GPIO_00, GpioPin.GPIO_25, GpioPin.GPIO_02, GpioPin.GPIO_03); + } + + /** + * Constructor. + * + * @param channel the {@link SpiChipSelect} to use. + * @param mode the {@link SpiMode} to use. + * @param speed the speed in Hz to use for communication. + * @param wake Used to wake the processor from a sleep mode. Active low. + * @param cs Chip select, active low, used as chip select/slave select on SPI + * @param reset Reset signal, active low, pull low to reset IC + * @param interrupt Interrupt, active low, pulls low when the BNO080 is ready for + * communication. + **/ + public Bno080SPIDevice(SpiChipSelect channel, SpiMode mode, int speed, GpioPin wake, GpioPin cs, GpioPin reset, + GpioPin interrupt) throws IOException, InterruptedException { + var pi4jRpiContext = Pi4J.newAutoContext(); + var spiConfig = Spi.newConfigBuilder(pi4jRpiContext).id("rpi-spi").chipSelect(channel).baud(speed).mode(mode) + .build(); + // spiDevice = new SpiDeviceImpl(channel, speed, mode); + spiDevice = pi4jRpiContext.spi().create(spiConfig); + configureSpiPins(pi4jRpiContext, wake, cs, reset, interrupt); + } + + public boolean isActive() { + return active.get(); + } + + @Override + public boolean start(SensorReportId report, int reportPeriod) { + if (reportPeriod > 0) { + final CountDownLatch latch = new CountDownLatch(1); + LOGGER.info("START: ready:{}, active:{}}", ready.get(), active.get()); + spiWaitCounter.set(0); + synchronized (executor) { + if (!ready.get()) { + initAndActive(latch, report, reportPeriod); + } else { + reactivate(latch, report, reportPeriod); + } + if (waitForLatch(latch)) { + executor.execute(() -> { + active.set(ready.get()); + executeListenerJob(); + }); + } + } + } else { + LOGGER.info("start: not valid sensor:{} delay:{}", report, reportPeriod); + } + return active.get(); + + } + + /** + * stop forces a soft reset command unit needs 700 millis to become to be + * available + * + * @return status + */ + @Override + public boolean stop() { + CountDownLatch latch = new CountDownLatch(1); + if (ready.get() && active.get()) { + active.set(false); + if (softReset()) { + try { + TimeUnit.MILLISECONDS.sleep(700); + } catch (InterruptedException e) { + throw new IllegalStateException("not possible stop"); + } + latch.countDown(); + return true; + } else { + LOGGER.debug("SOFT FALSE"); + } + } + return false; + } + + @Override + public void calibrate(long timeout) { + ShtpPacketRequest createCalibrateCommandAll = createCalibrateCommandAll(); + final CountDownLatch latch = new CountDownLatch(1); + executor.submit(() -> { + // TODO - track initialization better + try { + sendPacket(createCalibrateCommandAll); + } catch (InterruptedException | IOException e) { + LOGGER.error("Calibration failed!e:{}", e.getMessage(), e); + } + // TODO - wait for calibration to be good enough... + // TODO - stop calibration + latch.countDown(); + }); + } + + /** + * Calibration command. + */ + private ShtpPacketRequest createCalibrateCommandAll() { + ShtpChannel shtpChannel = ShtpChannel.COMMAND; + ShtpPacketRequest packet = prepareShtpPacketRequest(shtpChannel, 12); + packet.addBody(0, ControlReportId.COMMAND_REQUEST.getId()); + packet.addBody(0, commandSequenceNumber.getAndIncrement()); + packet.addBody(2, CommandId.ME_CALIBRATE.getId()); + packet.addBody(3, 1); + packet.addBody(4, 1); + packet.addBody(5, 1); + return packet; + } + + // private ShtpPacketRequest createSensorReportRequest(ControlReportId type, + // SensorReportId sensor) + // throws InterruptedException, IOException { + // ShtpChannel shtpChannel = ShtpChannel.CONTROL; + // ShtpPacketRequest packetRequest = prepareShtpPacketRequest(shtpChannel, + // 2); + // // //@formatter:off // int[] packetBody = new ShtpPacketBodyBuilder(packetRequest.getBodySize()) // .addElement(type.getId()) // .addElement(sensor.getId()) // .build(); // //@formatter:on - // packetRequest.addBody(packetBody); - // return packetRequest; - // } - - /** - * Get request to enable sensor report operation - * - * @param report - * sensor report to enable - * @param reportDelay - * time delay for sensor report - * @return operation head - */ - private ShtpOperation getSensorReportOperation(SensorReportId report, int reportDelay) { - ShtpOperationResponse response = new ShtpOperationResponse(ControlReportId.GET_FEATURE_RESPONSE); - ShtpPacketRequest request = createFeatureRequest(report, reportDelay, 0); - return new ShtpOperation(request, response); - } - - /** - * Enable device report - * - * @param report - * detailed report intent - * @param reportDelay - * necessary delay between the request and device response - * @return status - */ - private boolean enableSensorReport(SensorReportId report, int reportDelay) { - final ShtpOperation enableSensorReportOp = getSensorReportOperation(report, reportDelay); - try { - return processOperationChainByHead(enableSensorReportOp); - } catch (InterruptedException | IOException e) { - System.out.println("ERROR enableSensorReport:" + e.getMessage()); - return false; - } - } - - /** - * Process chain of defined operations. The operation represent - * {@link ShtpPacketRequest} and expected {@link ShtpOperationResponse}. It is - * possible that noise packet can be delivered in between, or packets that - * belongs to another listeners - * - * @param head - * initial operation in the operation chain - * @return status of the all process information - * @throws InterruptedException - * process has been interrupted - * @throws IOException - * IOException - */ - private boolean processOperationChainByHead(ShtpOperation head) throws InterruptedException, IOException { - int counter = 0; - boolean state = true; - do { - if (head.hasRequest()) { - sendPacket(head.getRequest()); - } - boolean waitForResponse = true; - while (waitForResponse && counter < MAX_COUNTER) { - ShtpPacketResponse response = receivePacket(false, RECEIVE_WRITE_BYTE); - ShtpOperationResponse opResponse = new ShtpOperationResponse( - ShtpChannel.getByChannel(response.getHeaderChannel()), response.getBodyFirst()); - if (head.getResponse().equals(opResponse)) { - waitForResponse = false; - } else { - waitForSPI(); - } - counter++; - } - head = head.getNext(); - if (state && counter >= MAX_COUNTER) { - state = false; - } - } while (head != null); - return state; - } - - /** - * receive packet and convert into DeviceEvent - * - * @return created event based on received date - */ - private DataEvent3f processReceivedPacket() { - try { - waitForSPI(); - ShtpPacketResponse receivedPacket = receivePacket(true, RECEIVE_WRITE_BYTE_CONTINUAL); - ShtpChannel channel = ShtpChannel.getByChannel(receivedPacket.getHeaderChannel()); - ShtpReportIds reportType = getReportType(channel, receivedPacket); - - switch (channel) { - case CONTROL: - break; - case REPORTS: - if (SensorReportId.BASE_TIMESTAMP.equals(reportType)) { - return parseInputReport(receivedPacket); - } - break; - default: - - } - System.out.printf("not implemented channel: %s, report: %s%n", channel, reportType); - return EMPTY_EVENT; - - } catch (IOException | InterruptedException e) { - System.out.println("ERROR: processReceivedPacket e:" + e.getMessage()); - return EMPTY_EVENT; - } - } - - private ShtpReportIds getReportType(ShtpChannel channel, ShtpPacketResponse response) { - switch (channel) { - case CONTROL: - return ControlReportId.getById(response.getBodyFirst()); - case REPORTS: - return SensorReportId.getById(response.getBodyFirst()); - default: - return ControlReportId.NONE; - } - } - - private boolean waitForLatch(CountDownLatch latch) { - try { - latch.await(DEFAULT_TIMEOUT_MS, TimeUnit.MILLISECONDS); - return true; - } catch (InterruptedException e) { - System.out.println("waitForLatch e: " + e.getMessage()); - return false; - } - } - - private ShtpOperation softResetSequence() { - ShtpPacketRequest request = getSoftResetPacket(); - return getInitSequence(request); - } - - /** - * Initiation operation sequence: 1. BNO080 is restarted and sends advertisement - * packet 2. BNO080 is requested for product id (product id) 3. BNO080 wait - * until reset is finished (reset response) - * - * @return head of operations - */ - private ShtpOperation getInitSequence(ShtpPacketRequest initRequest) { - ShtpOperationResponse advResponse = new ShtpOperationResponse(ShtpChannel.COMMAND, 0); - ShtpOperation headAdvertisementOp = new ShtpOperation(initRequest, advResponse); - ShtpOperationBuilder builder = new ShtpOperationBuilder(headAdvertisementOp); - - ShtpOperationResponse reportIdResponse = new ShtpOperationResponse(ControlReportId.PRODUCT_ID_RESPONSE); - ShtpOperation productIdOperation = new ShtpOperation(getProductIdRequest(), reportIdResponse); - builder.addOperation(productIdOperation); - - ShtpOperationResponse resetResponse = new ShtpOperationResponse(ControlReportId.COMMAND_RESPONSE); - ShtpOperation resetOperation = new ShtpOperation(null, resetResponse); - builder.addOperation(resetOperation); - - return builder.build(); - - } - - /** - * initiate soft reset - * - * @return state - */ - private boolean softReset() { - try { - ShtpOperation opHead = softResetSequence(); - return processOperationChainByHead(opHead); - } catch (InterruptedException | IOException e) { - System.out.printf("softReset error: %s%n", e.getMessage()); - } - return false; - } - - /** - * Initiate BNO080 unit and return the state - * - * @return BNO080 initial state - * @throws IOException - * @throws InterruptedException - */ - private boolean initiate() { - ShtpOperation opHead = getInitSequence(null); - try { - active.set(processOperationChainByHead(opHead)); - } catch (InterruptedException | IOException e) { - throw new IllegalStateException("Problem initializing device!"); - } - return active.get(); - } - - /** - * Get reported shtp errors - * - * @return receive number of errors - */ - public int getShtpError() { - ShtpPacketRequest errorRequest = getErrorRequest(); - - int errorCounts = -1; - try { - boolean active = true; - sendPacket(errorRequest); - while (active) { - ShtpPacketResponse response = receivePacket(false, RECEIVE_WRITE_BYTE); - ShtpChannel shtpChannel = ShtpChannel.getByChannel(response.getHeaderChannel()); - if (shtpChannel.equals(ShtpChannel.COMMAND) && (response.getBody()[0] == 0x01)) { - active = false; - // subtract - byte for the error. - errorCounts = response.getBody().length - 1; - } else { - TimeUnit.MICROSECONDS.sleep(UNIT_TICK_MICRO); - } - } - System.out.println("getShtpError: errorCounts=" + errorCounts); - return errorCounts; - } catch (IOException | InterruptedException e) { - e.printStackTrace(); - } - return errorCounts; - } - - /** - * SHTP packet contains 1 byte to get Error report. Packet is send to the - * COMMAND channel - * - * - * @return error request packet - */ - public ShtpPacketRequest getErrorRequest() { - ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.COMMAND, 1); - result.addBody(0, 0x01 & 0xFF); - return result; - } - - /** - * Configure SPI by Pins - * - * @param wake - * Active low, Used to wake the processor from a sleep mode. - * @param cs - * Chip select, active low, used as chip select/slave select on SPI - * @param reset - * Reset signal, active low, pull low to reset IC - * @param interrupt - * Interrupt, active low, pulls low when the BNO080 is ready for - * communication. - * @return process done - * @throws IOException - * exception - * @throws InterruptedException - * exception - */ - private boolean configureSpiPins(Context pi4jRpiContext, GpioPin wake, GpioPin cs, GpioPin reset, GpioPin interrupt) - throws InterruptedException { - System.out.println(String.format("configurePins: wak=%s, cs=%s, rst=%s, inter=%s", wake, cs, reset, interrupt)); - // GpioController gpioController = GpioFactory.getInstance(); - var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); - var csGpioConfig = digitalOutputBuilder.address(cs.address()).name("CS").build(); - var wakeGpioConfig = digitalOutputBuilder.address(wake.address()).onState(DigitalState.HIGH).build(); - // Pull up/down resistence is set to 2 - var intGpioConfig = digitalOutputBuilder.address(interrupt.address()).name(interrupt.name()).build(); - var rstGpioConfig = digitalOutputBuilder.address(reset.address()).onState(DigitalState.LOW).build(); - // csGpio = gpioController.provisionDigitalOutputPin(cs, "CS"); - // wakeGpio = gpioController.provisionDigitalOutputPin(wake); - // intGpio = gpioController.provisionDigitalInputPin(interrupt, - // PinPullResistance.PULL_UP); - // rstGpio = gpioController.provisionDigitalOutputPin(reset); - - csGpio = pi4jRpiContext.dout().create(csGpioConfig); - wakeGpio = pi4jRpiContext.dout().create(wakeGpioConfig); - intGpio = pi4jRpiContext.dout().create(intGpioConfig); - rstGpio = pi4jRpiContext.dout().create(rstGpioConfig); - - csGpio.setState(DigitalState.HIGH.value().intValue()); // Deselect BNO080 - - // Configure the BNO080 for SPI communication - // wakeGpio.setState(PinState.HIGH); // Before boot up the PS0/Wake - // rstGpio.setState(PinState.LOW); // Reset BNO080 - TimeUnit.SECONDS.sleep(2); // Min length not specified in datasheet? - rstGpio.setState(DigitalState.HIGH.value().intValue()); // Bring out of reset - return true; - } - - /** - * Unit responds with packet that contains the following: - */ - private DataEvent3f parseInputReport(ShtpPacketResponse packet) { - int[] payload = packet.getBody(); - - // Calculate the number of data bytes in this packet - final int dataLength = packet.getBodySize(); - long timeStamp = (payload[4] << 24) | (payload[3] << 16) | (payload[2] << 8) | (payload[1]); - - long accDelay = 17; - sensorReportDelayMicroSec = timeStamp + accDelay; - - int sensor = payload[5]; - int status = (payload[7] & 0x03) & 0xFF; // Get status bits - int data1 = ((payload[10] << 8) & 0xFFFF) | payload[9] & 0xFF; - int data2 = (payload[12] << 8 & 0xFFFF | (payload[11]) & 0xFF); - int data3 = (payload[14] << 8 & 0xFFFF) | (payload[13] & 0xFF); - int data4 = 0; - int data5 = 0; - - if (payload.length > 15 && dataLength - 5 > 9) { - data4 = (payload[16] & 0xFFFF) << 8 | payload[15] & 0xFF; - } - if (payload.length > 17 && dataLength - 5 > 11) { - data5 = (payload[18] & 0xFFFF) << 8 | payload[17] & 0xFF; - } - - final SensorReportId sensorReport = SensorReportId.getById(sensor); - - switch (sensorReport) { - case ACCELEROMETER: - return createDataEvent(DataEventType.ACCELEROMETER, timeStamp, status, data1, data2, data3, data4); - case RAW_ACCELEROMETER: - return createDataEvent(DataEventType.ACCELEROMETER_RAW, timeStamp, status, data1, data2, data3, data4); - case LINEAR_ACCELERATION: - return createDataEvent(DataEventType.ACCELEROMETER_LINEAR, timeStamp, status, data1, data2, data3, data4); - case GYROSCOPE: - return createDataEvent(DataEventType.GYROSCOPE, timeStamp, status, data1, data2, data3, data4); - case MAGNETIC_FIELD: - return createDataEvent(DataEventType.MAGNETOMETER, timeStamp, status, data1, data2, data3, data4); - case GAME_ROTATION_VECTOR: - return createVectorEvent(DataEventType.VECTOR_GAME, timeStamp, status, data1, data2, data3, data4, data5); - case ROTATION_VECTOR: - case GEOMAGNETIC_ROTATION_VECTOR: - return createVectorEvent(DataEventType.VECTOR_ROTATION, timeStamp, status, data1, data2, data3, data4, - data5); - default: - return EMPTY_EVENT; - } - } - - private DataEvent3f createVectorEvent(DataEventType type, long timeStamp, int... data) { - if (data == null || data.length < 6) { - return EMPTY_EVENT; - } - final int status = data[0] & 0xFFFF; - final int x = data[1] & 0xFFFF; - final int y = data[2] & 0xFFFF; - final int z = data[3] & 0xFFFF; - final int qReal = data[4] & 0xFFFF; - final int qRadianAccuracy = data[5] & 0xFFFF; // Only available on - // rotation vector, not - // game rot vector - final Tuple3f tuple3f = ShtpUtils.createTupleFromFixed(type.getQ(), x, y, z); - - return new VectorEvent(type, status, tuple3f, timeStamp, intToFloat(qReal, type.getQ()), - intToFloat(qRadianAccuracy, type.getQ())); - } - - private DataEvent3f createDataEvent(DataEventType type, long timeStamp, int... data) { - if (data == null || data.length < 4) { - return EMPTY_EVENT; - } - final int status = data[0] & 0xFFFF; - final int x = data[1] & 0xFFFF; - final int y = data[2] & 0xFFFF; - final int z = data[3] & 0xFFFF; - final Tuple3f tuple3f = ShtpUtils.createTupleFromFixed(type.getQ(), x, y, z); - return new DataEvent3f(type, status, tuple3f, timeStamp); - } - - /** - * Given a sensor's report ID, this tells the BNO080 to begin reporting the - * values Also sets the specific config word. Useful for personal activity - * classifier - * - * @param report - * - Shtp report received on Channel.REPORTS. - * @param timeBetweenReports - * - time between reports uint16_t - * @param specificConfig - * - contains specific config (uint32_t) - */ - private ShtpPacketRequest createFeatureRequest(SensorReportId report, int timeBetweenReports, int specificConfig) { - final long microsBetweenReports = timeBetweenReports * 1000L; - final ShtpPacketRequest request = prepareShtpPacketRequest(ShtpChannel.CONTROL, 17); - - //@formatter:off + // packetRequest.addBody(packetBody); + // return packetRequest; + // } + + /** + * Get request to enable sensor report operation + * + * @param report sensor report to enable + * @param reportDelay time delay for sensor report + * @return operation head + */ + private ShtpOperation getSensorReportOperation(SensorReportId report, int reportDelay) { + ShtpOperationResponse response = new ShtpOperationResponse(ControlReportId.GET_FEATURE_RESPONSE); + ShtpPacketRequest request = createFeatureRequest(report, reportDelay, 0); + return new ShtpOperation(request, response); + } + + /** + * Enable device report + * + * @param report detailed report intent + * @param reportDelay necessary delay between the request and device response + * @return status + */ + private boolean enableSensorReport(SensorReportId report, int reportDelay) { + final ShtpOperation enableSensorReportOp = getSensorReportOperation(report, reportDelay); + try { + return processOperationChainByHead(enableSensorReportOp); + } catch (InterruptedException | IOException e) { + LOGGER.error("ERROR enableSensorReport:{}", e.getMessage(), e); + return false; + } + } + + /** + * Process chain of defined operations. The operation represent + * {@link ShtpPacketRequest} and expected {@link ShtpOperationResponse}. It is + * possible that noise packet can be delivered in between, or packets that + * belongs to another listeners + * + * @param head initial operation in the operation chain + * @return status of the all process information + * @throws InterruptedException process has been interrupted + * @throws IOException IOException + */ + private boolean processOperationChainByHead(ShtpOperation head) throws InterruptedException, IOException { + int counter = 0; + boolean state = true; + do { + if (head.hasRequest()) { + sendPacket(head.getRequest()); + } + boolean waitForResponse = true; + while (waitForResponse && counter < MAX_COUNTER) { + ShtpPacketResponse response = receivePacket(false, RECEIVE_WRITE_BYTE); + ShtpOperationResponse opResponse = new ShtpOperationResponse( + ShtpChannel.getByChannel(response.getHeaderChannel()), response.getBodyFirst()); + if (head.getResponse().equals(opResponse)) { + waitForResponse = false; + } else { + waitForSPI(); + } + counter++; + } + head = head.getNext(); + if (state && counter >= MAX_COUNTER) { + state = false; + } + } while (head != null); + return state; + } + + /** + * receive packet and convert into DeviceEvent + * + * @return created event based on received date + */ + private DataEvent3f processReceivedPacket() { + try { + waitForSPI(); + ShtpPacketResponse receivedPacket = receivePacket(true, RECEIVE_WRITE_BYTE_CONTINUAL); + ShtpChannel channel = ShtpChannel.getByChannel(receivedPacket.getHeaderChannel()); + ShtpReportIds reportType = getReportType(channel, receivedPacket); + + switch (channel) { + case CONTROL: + break; + case REPORTS: + if (SensorReportId.BASE_TIMESTAMP.equals(reportType)) { + return parseInputReport(receivedPacket); + } + break; + default: + + } + LOGGER.debug("not implemented channel:{}, report:{}", channel, reportType); + return EMPTY_EVENT; + + } catch (IOException | InterruptedException e) { + LOGGER.error("ERROR: processReceivedPacket e:{}", e.getMessage(), e); + return EMPTY_EVENT; + } + } + + private ShtpReportIds getReportType(ShtpChannel channel, ShtpPacketResponse response) { + switch (channel) { + case CONTROL: + return ControlReportId.getById(response.getBodyFirst()); + case REPORTS: + return SensorReportId.getById(response.getBodyFirst()); + default: + return ControlReportId.NONE; + } + } + + private boolean waitForLatch(CountDownLatch latch) { + try { + latch.await(DEFAULT_TIMEOUT_MS, TimeUnit.MILLISECONDS); + return true; + } catch (InterruptedException e) { + LOGGER.debug("waitForLatch e: {}", e.getMessage()); + return false; + } + } + + private ShtpOperation softResetSequence() { + ShtpPacketRequest request = getSoftResetPacket(); + return getInitSequence(request); + } + + /** + * Initiation operation sequence: 1. BNO080 is restarted and sends advertisement + * packet 2. BNO080 is requested for product id (product id) 3. BNO080 wait + * until reset is finished (reset response) + * + * @return head of operations + */ + private ShtpOperation getInitSequence(ShtpPacketRequest initRequest) { + ShtpOperationResponse advResponse = new ShtpOperationResponse(ShtpChannel.COMMAND, 0); + ShtpOperation headAdvertisementOp = new ShtpOperation(initRequest, advResponse); + ShtpOperationBuilder builder = new ShtpOperationBuilder(headAdvertisementOp); + + ShtpOperationResponse reportIdResponse = new ShtpOperationResponse(ControlReportId.PRODUCT_ID_RESPONSE); + ShtpOperation productIdOperation = new ShtpOperation(getProductIdRequest(), reportIdResponse); + builder.addOperation(productIdOperation); + + ShtpOperationResponse resetResponse = new ShtpOperationResponse(ControlReportId.COMMAND_RESPONSE); + ShtpOperation resetOperation = new ShtpOperation(null, resetResponse); + builder.addOperation(resetOperation); + + return builder.build(); + + } + + /** + * initiate soft reset + * + * @return state + */ + private boolean softReset() { + try { + ShtpOperation opHead = softResetSequence(); + return processOperationChainByHead(opHead); + } catch (InterruptedException | IOException e) { + LOGGER.error("softReset error:{}", e.getMessage(), e); + } + return false; + } + + /** + * Initiate BNO080 unit and return the state + * + * @return BNO080 initial state + */ + private boolean initiate() { + ShtpOperation opHead = getInitSequence(null); + try { + active.set(processOperationChainByHead(opHead)); + } catch (InterruptedException | IOException e) { + throw new IllegalStateException("Problem initializing device!"); + } + return active.get(); + } + + /** + * Get reported shtp errors + * + * @return receive number of errors + */ + public int getShtpError() { + ShtpPacketRequest errorRequest = getErrorRequest(); + + int errorCounts = -1; + try { + boolean active = true; + sendPacket(errorRequest); + while (active) { + ShtpPacketResponse response = receivePacket(false, RECEIVE_WRITE_BYTE); + ShtpChannel shtpChannel = ShtpChannel.getByChannel(response.getHeaderChannel()); + if (shtpChannel.equals(ShtpChannel.COMMAND) && (response.getBody()[0] == 0x01)) { + active = false; + // subtract - byte for the error. + errorCounts = response.getBody().length - 1; + } else { + TimeUnit.MICROSECONDS.sleep(UNIT_TICK_MICRO); + } + } + LOGGER.debug("getShtpError: errorCounts={}", errorCounts); + return errorCounts; + } catch (IOException | InterruptedException e) { + LOGGER.error("getShtpError:{}", e.getMessage(), e); + } + return errorCounts; + } + + /** + * SHTP packet contains 1 byte to get Error report. Packet is send to the + * COMMAND channel + * + * @return error request packet + */ + public ShtpPacketRequest getErrorRequest() { + ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.COMMAND, 1); + result.addBody(0, 0x01 & 0xFF); + return result; + } + + /** + * Configure SPI by Pins + * + * @param wake Active low, Used to wake the processor from a sleep mode. + * @param cs Chip select, active low, used as chip select/slave select on SPI + * @param reset Reset signal, active low, pull low to reset IC + * @param interrupt Interrupt, active low, pulls low when the BNO080 is ready for + * communication. + * @throws InterruptedException exception + */ + private void configureSpiPins(Context pi4jRpiContext, GpioPin wake, GpioPin cs, GpioPin reset, GpioPin interrupt) + throws InterruptedException { + LOGGER.info("configurePins: wak={}, cs={}, rst={}, inter={}", wake, cs, reset, interrupt); + // GpioController gpioController = GpioFactory.getInstance(); + var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); + var csGpioConfig = digitalOutputBuilder.address(cs.address()).name("CS").build(); + var wakeGpioConfig = digitalOutputBuilder.address(wake.address()).onState(DigitalState.HIGH).build(); + // Pull up/down resistence is set to 2 + var intGpioConfig = digitalOutputBuilder.address(interrupt.address()).name(interrupt.name()).build(); + var rstGpioConfig = digitalOutputBuilder.address(reset.address()).onState(DigitalState.LOW).build(); + // csGpio = gpioController.provisionDigitalOutputPin(cs, "CS"); + // wakeGpio = gpioController.provisionDigitalOutputPin(wake); + // intGpio = gpioController.provisionDigitalInputPin(interrupt, + // PinPullResistance.PULL_UP); + // rstGpio = gpioController.provisionDigitalOutputPin(reset); + + csGpio = pi4jRpiContext.dout().create(csGpioConfig); + wakeGpio = pi4jRpiContext.dout().create(wakeGpioConfig); + intGpio = pi4jRpiContext.dout().create(intGpioConfig); + rstGpio = pi4jRpiContext.dout().create(rstGpioConfig); + + csGpio.setState(DigitalState.HIGH.value().intValue()); // Deselect BNO080 + + // Configure the BNO080 for SPI communication + // wakeGpio.setState(PinState.HIGH); // Before boot up the PS0/Wake + // rstGpio.setState(PinState.LOW); // Reset BNO080 + TimeUnit.SECONDS.sleep(2); // Min length not specified in datasheet? + rstGpio.setState(DigitalState.HIGH.value().intValue()); // Bring out of reset + } + + /** + * Unit responds with packet that contains the following: + */ + private DataEvent3f parseInputReport(ShtpPacketResponse packet) { + int[] payload = packet.getBody(); + + // Calculate the number of data bytes in this packet + final int dataLength = packet.getBodySize(); + long timeStamp = (payload[4] << 24) | (payload[3] << 16) | (payload[2] << 8) | (payload[1]); + + long accDelay = 17; + sensorReportDelayMicroSec = timeStamp + accDelay; + + int sensor = payload[5]; + int status = (payload[7] & 0x03) & 0xFF; // Get status bits + int data1 = ((payload[10] << 8) & 0xFFFF) | payload[9] & 0xFF; + int data2 = (payload[12] << 8 & 0xFFFF | (payload[11]) & 0xFF); + int data3 = (payload[14] << 8 & 0xFFFF) | (payload[13] & 0xFF); + int data4 = 0; + int data5 = 0; + + if (payload.length > 15 && dataLength - 5 > 9) { + data4 = (payload[16] & 0xFFFF) << 8 | payload[15] & 0xFF; + } + if (payload.length > 17 && dataLength - 5 > 11) { + data5 = (payload[18] & 0xFFFF) << 8 | payload[17] & 0xFF; + } + + final SensorReportId sensorReport = SensorReportId.getById(sensor); + + switch (sensorReport) { + case ACCELEROMETER: + return createDataEvent(DataEventType.ACCELEROMETER, timeStamp, status, data1, data2, data3, data4); + case RAW_ACCELEROMETER: + return createDataEvent(DataEventType.ACCELEROMETER_RAW, timeStamp, status, data1, data2, data3, data4); + case LINEAR_ACCELERATION: + return createDataEvent(DataEventType.ACCELEROMETER_LINEAR, timeStamp, status, data1, data2, data3, data4); + case GYROSCOPE: + return createDataEvent(DataEventType.GYROSCOPE, timeStamp, status, data1, data2, data3, data4); + case MAGNETIC_FIELD: + return createDataEvent(DataEventType.MAGNETOMETER, timeStamp, status, data1, data2, data3, data4); + case GAME_ROTATION_VECTOR: + return createVectorEvent(DataEventType.VECTOR_GAME, timeStamp, status, data1, data2, data3, data4, data5); + case ROTATION_VECTOR: + case GEOMAGNETIC_ROTATION_VECTOR: + return createVectorEvent(DataEventType.VECTOR_ROTATION, timeStamp, status, data1, data2, data3, data4, + data5); + default: + return EMPTY_EVENT; + } + } + + private DataEvent3f createVectorEvent(DataEventType type, long timeStamp, int... data) { + if (data == null || data.length < 6) { + return EMPTY_EVENT; + } + final int status = data[0] & 0xFFFF; + final int x = data[1] & 0xFFFF; + final int y = data[2] & 0xFFFF; + final int z = data[3] & 0xFFFF; + final int qReal = data[4] & 0xFFFF; + final int qRadianAccuracy = data[5] & 0xFFFF; // Only available on + // rotation vector, not + // game rot vector + final Tuple3f tuple3f = ShtpUtils.createTupleFromFixed(type.getQ(), x, y, z); + + return new VectorEvent(type, status, tuple3f, timeStamp, intToFloat(qReal, type.getQ()), + intToFloat(qRadianAccuracy, type.getQ())); + } + + private DataEvent3f createDataEvent(DataEventType type, long timeStamp, int... data) { + if (data == null || data.length < 4) { + return EMPTY_EVENT; + } + final int status = data[0] & 0xFFFF; + final int x = data[1] & 0xFFFF; + final int y = data[2] & 0xFFFF; + final int z = data[3] & 0xFFFF; + final Tuple3f tuple3f = ShtpUtils.createTupleFromFixed(type.getQ(), x, y, z); + return new DataEvent3f(type, status, tuple3f, timeStamp); + } + + /** + * Given a sensor's report ID, this tells the BNO080 to begin reporting the + * values Also sets the specific config word. Useful for personal activity + * classifier + * + * @param report - Shtp report received on Channel.REPORTS. + * @param timeBetweenReports - time between reports uint16_t + * @param specificConfig - contains specific config (uint32_t) + */ + private ShtpPacketRequest createFeatureRequest(SensorReportId report, int timeBetweenReports, int specificConfig) { + final long microsBetweenReports = timeBetweenReports * 1000L; + final ShtpPacketRequest request = prepareShtpPacketRequest(ShtpChannel.CONTROL, 17); + + //@formatter:off int[] packetBody = new ShtpPacketBodyBuilder(request.getBodySize()) .addElement(ControlReportId.SET_FEATURE_COMMAND.getId()) .addElement(report.getId()) // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector @@ -665,146 +622,146 @@ private ShtpPacketRequest createFeatureRequest(SensorReportId report, int timeBe .build(); request.addBody(packetBody); //@formatter:on - return request; - } - - /** - * Blocking wait for BNO080 to assert (pull low) the INT pin indicating it's - * ready for comm. Can take more than 200ms after a hardware reset - */ - private boolean waitForSPI() throws InterruptedException { - int counter = 0; - for (int i = 0; i < MAX_SPI_COUNT; i++) { - if (intGpio.isLow()) { - return true; - } else { - counter++; - } - TimeUnit.MICROSECONDS.sleep(1); - } - if (spiWaitCounter.getAndIncrement() == MAX_SPI_WAIT_CYCLES) { - stop(); - } - System.out.println("waitForSPI failed. Counter: " + counter); - return false; - } - - private boolean sendPacket(ShtpPacketRequest packet) throws InterruptedException, IOException { - // Wait for BNO080 to indicate it is available for communication - if (!waitForSPI()) { - System.out.println("sendPacket SPI not available for communication"); - return false; - } - - // BNO080 has max CLK of 3MHz, MSB first, - // The BNO080 uses CPOL = 1 and CPHA = 1. This is mode3 - // csGpio.setState(PinState.LOW); - csGpio.setState(DigitalState.LOW.value().intValue()); - - for (int i = 0; i < packet.getHeaderSize(); i++) { - spiDevice.write(packet.getHeaderByte(i)); - } - - for (int i = 0; i < packet.getBodySize(); i++) { - spiDevice.write(packet.getBodyByte(i)); - } - // csGpio.setState(PinState.HIGH); - csGpio.setState(DigitalState.HIGH.value().intValue()); - return true; - } - - private ShtpPacketResponse receivePacket(boolean delay, byte writeByte) throws IOException, InterruptedException { - if (intGpio.isHigh()) { - System.out.println("receivedPacketContinual: no interrupt"); - return new ShtpPacketResponse(0); - } - // Get first four bytes to find out how much data we need to read - if (delay && sensorReportDelayMicroSec > 0) { - TimeUnit.MICROSECONDS.sleep(TIMEBASE_REFER_DELTA - sensorReportDelayMicroSec); - } - // csGpio.setState(PinState.LOW); - csGpio.setState(DigitalState.LOW.value().intValue()); - - final byte[] writeBuffer1 = new byte[1]; - writeBuffer1[0] = writeByte; - spiDevice.write(writeBuffer1); - // int packetLSB = toInt8U(spiDevice.write(writeByte)); - int packetLSB = toInt8U(writeBuffer1); - // int packetMSB = toInt8U(spiDevice.write(writeByte)); - int packetMSB = toInt8U(writeBuffer1); - // int channelNumber = toInt8U(spiDevice.write(writeByte)); - int channelNumber = toInt8U(writeBuffer1); - // int sequenceNumber = toInt8U(spiDevice.write(writeByte)); - int sequenceNumber = toInt8U(writeBuffer1); - // Calculate the number of data bytes in this packet - int dataLength = calculateNumberOfBytesInPacket(packetMSB, packetLSB) - SHTP_HEADER_SIZE; - if (dataLength <= 0) { - return new ShtpPacketResponse(0); - } - - ShtpPacketResponse response = new ShtpPacketResponse(dataLength); - response.addHeader(packetLSB, packetMSB, channelNumber, sequenceNumber); - - for (int i = 0; i < dataLength; i++) { - byte[] writeBuffer2 = new byte[1]; - writeBuffer2[0] = (byte) 0xFF; - // byte[] inArray = spiDevice.write((byte) 0xFF); - spiDevice.write(writeBuffer2); - byte incoming = writeBuffer2[0]; - if (i < MAX_PACKET_SIZE) { - response.addBody(i, (incoming & 0xFF)); - } - } - // Get first four bytes to find out how much data we need to read - // csGpio.setState(PinState.HIGH); - csGpio.setState(DigitalState.HIGH.value().intValue()); - - return response; - } - - private void initAndActive(final CountDownLatch latch, SensorReportId report, int reportPeriod) { - executor.submit(() -> { - boolean initState = initiate(); - if (initState && enableSensorReport(report, reportPeriod)) { - latch.countDown(); - ready.set(initState); - } - }); - } - - private void reactivate(final CountDownLatch latch, SensorReportId report, int reportPeriod) { - executor.submit(() -> { - try { - ShtpOperation opHead = getInitSequence(null); - active.set(processOperationChainByHead(opHead)); - if (active.get() && enableSensorReport(report, reportPeriod)) { - latch.countDown(); - } - } catch (InterruptedException | IOException e) { - throw new IllegalStateException("not activated"); - } - }); - } - - private void executeListenerJob() { - executor.execute(() -> { - if (ready.get()) { - active.set(ready.get()); - while (active.get()) { - forwardReceivedPacketToListeners(); - } - } else { - throw new IllegalStateException("not initiated"); - } - }); - } - - private void forwardReceivedPacketToListeners() { - DataEvent3f deviceEvent = processReceivedPacket(); - if (!deviceEvent.getType().equals(DataEventType.NONE)) { - for (DataListener l : listeners) { - l.onResponse(deviceEvent); - } - } - } + return request; + } + + /** + * Blocking wait for BNO080 to assert (pull low) the INT pin indicating it's + * ready for comm. Can take more than 200ms after a hardware reset + */ + private boolean waitForSPI() throws InterruptedException { + int counter = 0; + for (int i = 0; i < MAX_SPI_COUNT; i++) { + if (intGpio.isLow()) { + return true; + } else { + counter++; + } + TimeUnit.MICROSECONDS.sleep(1); + } + if (spiWaitCounter.getAndIncrement() == MAX_SPI_WAIT_CYCLES) { + stop(); + } + LOGGER.debug("waitForSPI failed. Counter: {}", counter); + return false; + } + + private boolean sendPacket(ShtpPacketRequest packet) throws InterruptedException, IOException { + // Wait for BNO080 to indicate it is available for communication + if (!waitForSPI()) { + LOGGER.debug("sendPacket SPI not available for communication"); + return false; + } + + // BNO080 has max CLK of 3MHz, MSB first, + // The BNO080 uses CPOL = 1 and CPHA = 1. This is mode3 + // csGpio.setState(PinState.LOW); + csGpio.setState(DigitalState.LOW.value().intValue()); + + for (int i = 0; i < packet.getHeaderSize(); i++) { + spiDevice.write(packet.getHeaderByte(i)); + } + + for (int i = 0; i < packet.getBodySize(); i++) { + spiDevice.write(packet.getBodyByte(i)); + } + // csGpio.setState(PinState.HIGH); + csGpio.setState(DigitalState.HIGH.value().intValue()); + return true; + } + + private ShtpPacketResponse receivePacket(boolean delay, byte writeByte) throws IOException, InterruptedException { + if (intGpio.isHigh()) { + LOGGER.debug("receivedPacketContinual: no interrupt"); + return new ShtpPacketResponse(0); + } + // Get first four bytes to find out how much data we need to read + if (delay && sensorReportDelayMicroSec > 0) { + TimeUnit.MICROSECONDS.sleep(TIMEBASE_REFER_DELTA - sensorReportDelayMicroSec); + } + // csGpio.setState(PinState.LOW); + csGpio.setState(DigitalState.LOW.value().intValue()); + + final byte[] writeBuffer1 = new byte[1]; + writeBuffer1[0] = writeByte; + spiDevice.write(writeBuffer1); + // int packetLSB = toInt8U(spiDevice.write(writeByte)); + int packetLSB = toInt8U(writeBuffer1); + // int packetMSB = toInt8U(spiDevice.write(writeByte)); + int packetMSB = toInt8U(writeBuffer1); + // int channelNumber = toInt8U(spiDevice.write(writeByte)); + int channelNumber = toInt8U(writeBuffer1); + // int sequenceNumber = toInt8U(spiDevice.write(writeByte)); + int sequenceNumber = toInt8U(writeBuffer1); + // Calculate the number of data bytes in this packet + int dataLength = calculateNumberOfBytesInPacket(packetMSB, packetLSB) - SHTP_HEADER_SIZE; + if (dataLength <= 0) { + return new ShtpPacketResponse(0); + } + + ShtpPacketResponse response = new ShtpPacketResponse(dataLength); + response.addHeader(packetLSB, packetMSB, channelNumber, sequenceNumber); + + for (int i = 0; i < dataLength; i++) { + byte[] writeBuffer2 = new byte[1]; + writeBuffer2[0] = (byte) 0xFF; + // byte[] inArray = spiDevice.write((byte) 0xFF); + spiDevice.write(writeBuffer2); + byte incoming = writeBuffer2[0]; + if (i < MAX_PACKET_SIZE) { + response.addBody(i, (incoming & 0xFF)); + } + } + // Get first four bytes to find out how much data we need to read + // csGpio.setState(PinState.HIGH); + csGpio.setState(DigitalState.HIGH.value().intValue()); + + return response; + } + + private void initAndActive(final CountDownLatch latch, SensorReportId report, int reportPeriod) { + executor.submit(() -> { + boolean initState = initiate(); + if (initState && enableSensorReport(report, reportPeriod)) { + latch.countDown(); + ready.set(initState); + } + }); + } + + private void reactivate(final CountDownLatch latch, SensorReportId report, int reportPeriod) { + executor.submit(() -> { + try { + ShtpOperation opHead = getInitSequence(null); + active.set(processOperationChainByHead(opHead)); + if (active.get() && enableSensorReport(report, reportPeriod)) { + latch.countDown(); + } + } catch (InterruptedException | IOException e) { + throw new IllegalStateException("not activated"); + } + }); + } + + private void executeListenerJob() { + executor.execute(() -> { + if (ready.get()) { + active.set(ready.get()); + while (active.get()) { + forwardReceivedPacketToListeners(); + } + } else { + throw new IllegalStateException("not initiated"); + } + }); + } + + private void forwardReceivedPacketToListeners() { + DataEvent3f deviceEvent = processReceivedPacket(); + if (!deviceEvent.getType().equals(DataEventType.NONE)) { + for (DataListener l : listeners) { + l.onResponse(deviceEvent); + } + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketResponse.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketResponse.java index f7bc9c12..9f3e99ef 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketResponse.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketResponse.java @@ -17,6 +17,10 @@ package com.robo4j.hw.rpi.imu.bno.shtp; +import com.robo4j.hw.rpi.imu.bno.impl.Bno080SPIDevice; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import static com.robo4j.hw.rpi.imu.bno.impl.Bno080SPIDevice.SHTP_HEADER_SIZE; /** @@ -27,8 +31,10 @@ * @author Miroslav Wengner (@miragemiko) */ public final class ShtpPacketResponse { + private static final Logger LOGGER = LoggerFactory.getLogger(ShtpPacketResponse.class); + private final int[] header = new int[SHTP_HEADER_SIZE]; - private int[] body; + private final int[] body; public ShtpPacketResponse(int size) { this.body = new int[size]; @@ -36,7 +42,7 @@ public ShtpPacketResponse(int size) { public void addHeader(int... header) { if (header.length != this.header.length) { - System.out.println("ShtpPacketRequest: wrong header"); + LOGGER.debug("ShtpPacketRequest: wrong header"); } else { this.header[0] = header[0]; // LSB this.header[1] = header[1]; // MSB diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java index a3095431..4f4f1054 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java @@ -21,6 +21,8 @@ import com.robo4j.hw.rpi.imu.bno.DataEventType; import com.robo4j.hw.rpi.imu.bno.impl.AbstractBno080Device.CommandId; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * ShtpUtils collection of useful utils @@ -29,177 +31,148 @@ * @author Miroslav Wengner (@miragemiko) */ public final class ShtpUtils { + private static final Logger LOGGER = LoggerFactory.getLogger(ShtpUtils.class); + public static final String EMPTY = ""; + /** + * Empty Shtp Device Event + */ + public static final DataEvent3f EMPTY_EVENT = new DataEvent3f(DataEventType.NONE, 0, null, 0); - /** - * Empty Shtp Device Event - */ - public static final DataEvent3f EMPTY_EVENT = new DataEvent3f(DataEventType.NONE, 0, null, 0); + /** + * Given a register value and a Q point, convert to float See + * https://en.wikipedia.org/wiki/Q_(number_format) + * + * @param fixedPointValue fixed point value + * @param qPoint q point + * @return float value + */ + public static float intToFloat(int fixedPointValue, int qPoint) { + float qFloat = (short) fixedPointValue; + qFloat *= Math.pow(2, (qPoint & 0xFF) * -1); + return qFloat; + } - /** - * Given a register value and a Q point, convert to float See - * https://en.wikipedia.org/wiki/Q_(number_format) - * - * @param fixedPointValue - * fixed point value - * @param qPoint - * q point - * @return float value - */ - public static float intToFloat(int fixedPointValue, int qPoint) { - float qFloat = (short) fixedPointValue; - qFloat *= Math.pow(2, (qPoint & 0xFF) * -1); - return qFloat; - } + /** + * Pretty prints an array of ints representing unsigned bytes as hex. + * + * @param array the array to pretty print. + * @return the string representing the pretty printed array. + */ + public static String toHexString(int[] array) { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < array.length - 1; i++) { + builder.append(Integer.toHexString(array[i] & 0xFF)); + builder.append(", "); + } + builder.append(Integer.toHexString(array[array.length - 1] & 0xFF)); + return builder.toString(); + } - /** - * Pretty prints an array of ints representing unsigned bytes as hex. - * - * @param array - * the array to pretty print. - * @return the string representing the pretty printed array. - */ - public static String toHexString(int[] array) { - StringBuilder builder = new StringBuilder(); - for (int i = 0; i < array.length - 1; i++) { - builder.append(Integer.toHexString(array[i] & 0xFF)); - builder.append(", "); - } - builder.append(Integer.toHexString(array[array.length - 1] & 0xFF)); - return builder.toString(); - } + /** + * Calculate packet length. + * + * @param packetMSB uint8 + * @param packetLSB uint8 + * @return integer size + */ + public static int calculateNumberOfBytesInPacket(int packetMSB, int packetLSB) { + // Calculate the number of data bytes in this packet + int dataLength = (0xFFFF & packetMSB << 8 | packetLSB); + dataLength &= ~(1 << 15); // Clear the MSbit. + return dataLength; + } - /** - * Calculate packet length. - * - * @param packetMSB - * uint8 - * @param packetLSB - * uint8 - * @return integer size - */ - public static int calculateNumberOfBytesInPacket(int packetMSB, int packetLSB) { - // Calculate the number of data bytes in this packet - int dataLength = (0xFFFF & packetMSB << 8 | packetLSB); - dataLength &= ~(1 << 15); // Clear the MSbit. - return dataLength; - } + /** + * Reads the first byte in the array as an unsigned byte, represented as an + * int. + * + * @param array byte array from which to read the first element. + * @return the first byte in the array read as an unsigned byte, represented + * as an int. + */ + public static int toInt8U(byte[] array) { + return array[0] & 0xFF; + } - /** - * Reads the first byte in the array as an unsigned byte, represented as an - * int. - * - * @param array - * byte array from which to read the first element. - * @return the first byte in the array read as an unsigned byte, represented - * as an int. - */ - public static int toInt8U(byte[] array) { - return array[0] & 0xFF; - } + /** + * Print hexadecimal values of int array to system out. + * + * @param message message + * @param array array int values + */ + public static void debugPrintArray(String message, int[] array) { + LOGGER.debug("printArray:{}", message); + LOGGER.debug(toHexString(array)); + LOGGER.debug(EMPTY); + } - /** - * Print hexadecimal values of int array to system out. - * - * @param message - * message - * @param array - * array int values - */ - public static void debugPrintArray(String message, int[] array) { - System.out.print("printArray: " + message); - System.out.print(toHexString(array)); - System.out.println(""); - } + /** + * Creates a float tuple from data provided in fixed int format. + * + * @param qPoint qPoint + * @param fixedX fixedX + * @param fixedY fixedY + * @param fixedZ fixedZ + * @return the created float tuple. + */ + public static Tuple3f createTupleFromFixed(int qPoint, int fixedX, int fixedY, int fixedZ) { + float x = ShtpUtils.intToFloat(fixedX, qPoint); + float y = ShtpUtils.intToFloat(fixedY, qPoint); + float z = ShtpUtils.intToFloat(fixedZ, qPoint); + return new Tuple3f(x, y, z); + } - /** - * Creates a float tuple from data provided in fixed int format. - * - * @param qPoint - * @param fixedX - * @param fixedY - * @param fixedZ - * @return the created float tuple. - */ - public static Tuple3f createTupleFromFixed(int qPoint, int fixedX, int fixedY, int fixedZ) { - float x = ShtpUtils.intToFloat(fixedX, qPoint); - float y = ShtpUtils.intToFloat(fixedY, qPoint); - float z = ShtpUtils.intToFloat(fixedZ, qPoint); - return new Tuple3f(x, y, z); - } + /** + * Pretty prints a packet. + */ + public static void debugPrintShtpPacket(ControlReportId reportId, String prefix, int[] data) { + switch (reportId) { + case PRODUCT_ID_RESPONSE: + LOGGER.debug("printShtpPacketPart:{}:report={}:value={}", prefix, reportId, Integer.toHexString(data[0])); + break; + default: + LOGGER.debug("printShtpPacketPart:{}:NO IMPL={}:value={}", prefix, reportId, Integer.toHexString(data[0])); + } + for (int i = 0; i < data.length; i++) { + LOGGER.debug("printShtpPacketPart{}::[{}]:{}", prefix, i, Integer.toHexString(data[i])); + } + } - /** - * Pretty prints a packet. - */ - public static void debugPrintShtpPacket(ControlReportId reportId, String prefix, int[] data) { - switch (reportId) { - case PRODUCT_ID_RESPONSE: - System.out.println(String.format("printShtpPacketPart:%s:report=%s:value=%s", prefix, reportId, Integer.toHexString(data[0]))); - break; - default: - System.out.println(String.format("printShtpPacketPart:%s:NO IMPL=%s:value=%s", prefix, reportId, Integer.toHexString(data[0]))); - } - for (int i = 0; i < data.length; i++) { - System.out.println("printShtpPacketPart" + prefix + "::[" + i + "]:" + Integer.toHexString(data[i])); - } - } + /** + * The BNO080 responds with this report to command requests. It's up to use + * to remember which command we issued + * + * @param commandId device command in response + */ + public static void debugPringCommandResponse(CommandId commandId, int[] payload) { + switch (commandId) { + case ME_CALIBRATE: + // R0 - Status (0 = success, non-zero = fail) + int calibrationStatus = payload[10] & 0xFF; + LOGGER.debug("parseInputReport: command response: command= {} calibrationStatus= {}", commandId, calibrationStatus); + break; + case COUNTER, TARE, INITIALIZE, DCD, DCD_PERIOD_SAVE, OSCILLATOR, CLEAR_DCD, ERRORS: + LOGGER.debug("parseInputReport: deviceCommand={}", commandId); + break; + default: + LOGGER.debug("parseInputReport: not available deviceCommand={}", commandId); + break; + } + } - /** - * The BNO080 responds with this report to command requests. It's up to use - * to remember which command we issued - * - * @param commandId - * device command in response - */ - public static void debugPringCommandResponse(CommandId commandId, int[] payload) { - switch (commandId) { - case ERRORS: - System.out.println("parseInputReport: deviceCommand=" + commandId); - break; - case COUNTER: - System.out.println("parseInputReport: COUNTER deviceCommand=" + commandId); - break; - case TARE: - System.out.println("parseInputReport: TARE deviceCommand=" + commandId); - break; - case INITIALIZE: - System.out.println("parseInputReport: INITIALIZE deviceCommand=" + commandId); - break; - case DCD: - System.out.println("parseInputReport: DCD deviceCommand=" + commandId); - break; - case ME_CALIBRATE: - // R0 - Status (0 = success, non-zero = fail) - int calibrationStatus = payload[10] & 0xFF; - System.out.println("parseInputReport: command response: command= " + commandId + " calibrationStatus= " + calibrationStatus); - break; - case DCD_PERIOD_SAVE: - System.out.println("parseInputReport: deviceCommand=" + commandId); - break; - case OSCILLATOR: - System.out.println("parseInputReport: deviceCommand=" + commandId); - break; - case CLEAR_DCD: - System.out.println("parseInputReport: deviceCommand=" + commandId); - break; - default: - System.out.println("parseInputReport: not available deviceCommand=" + commandId); - break; - } - } - - public static void debugPrintCommandReport(ShtpPacketResponse packet) { - int[] payload = packet.getBody(); - ControlReportId report = ControlReportId.getById(payload[0] & 0xFF); - if (report.equals(ControlReportId.COMMAND_RESPONSE)) { - // The BNO080 responds with this report to command requests. It's up - // to use to remember which command we issued. - CommandId command = CommandId.getById(payload[2] & 0xFF); - System.out.println("parseCommandReport: commandResponse: " + command); - if (CommandId.ME_CALIBRATE.equals(command)) { - System.out.println("Calibration status" + (payload[5] & 0xFF)); - } - } else { - System.out.println("parseCommandReport: This sensor report ID is unhandled"); - } - } + public static void debugPrintCommandReport(ShtpPacketResponse packet) { + int[] payload = packet.getBody(); + ControlReportId report = ControlReportId.getById(payload[0] & 0xFF); + if (report.equals(ControlReportId.COMMAND_RESPONSE)) { + // The BNO080 responds with this report to command requests. It's up + // to use to remember which command we issued. + CommandId command = CommandId.getById(payload[2] & 0xFF); + LOGGER.debug("parseCommandReport: commandResponse: {}", command); + if (CommandId.ME_CALIBRATE.equals(command)) { + LOGGER.debug("Calibration status{}", payload[5] & 0xFF); + } + } else { + LOGGER.debug("parseCommandReport: This sensor report ID is unhandled"); + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/Lcd20x4.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/Lcd20x4.java index af5427a9..913c320f 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/Lcd20x4.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/Lcd20x4.java @@ -22,22 +22,25 @@ import com.pi4j.io.gpio.digital.DigitalOutputConfigBuilder; import com.pi4j.io.gpio.digital.DigitalState; import com.robo4j.hw.rpi.utils.GpioPin; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.TimeUnit; /** * Hardware support for the 20x4 LCD module. - * + *

* See * http://www.raspberrypi-spy.co.uk/2012/08/20x4-lcd-module-control-using-python/ - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class Lcd20x4 { - private final static int CHAR_WIDTH = 20; + private static final Logger LOGGER = LoggerFactory.getLogger(Lcd20x4.class); + private final static int CHAR_WIDTH = 20; - //@formatter:off + //@formatter:off /** * From the webpage: * @@ -67,83 +70,76 @@ public class Lcd20x4 { // private final GpioPinDigitalOutput gpioD6; // private final GpioPinDigitalOutput gpioD7; // private final GpioPinDigitalOutput gpioOn; - private final DigitalOutput gpioRS; - private final DigitalOutput gpioE; - private final DigitalOutput gpioD4; - private final DigitalOutput gpioD5; - private final DigitalOutput gpioD6; - private final DigitalOutput gpioD7; - private final DigitalOutput gpioOn; - - // Delay in nanos - private final static int E_DELAY = (int) TimeUnit.MICROSECONDS.toNanos(500); - - private enum Mode { - CMD(false), CHAR(true); - - private final boolean sendValue; - - Mode(boolean sendValue) { - this.sendValue = sendValue; - } - - public boolean getSendValue() { - return sendValue; - } - } - - public enum Alignment { - LEFT, CENTER, RIGHT - } - - /** - * Default constructor. - * - * Use this if you have used the default wiring in the example. - */ - public Lcd20x4() { - this(GpioPin.GPIO_11, GpioPin.GPIO_10, GpioPin.GPIO_06, GpioPin.GPIO_05, GpioPin.GPIO_04, GpioPin.GPIO_01, - GpioPin.GPIO_16); - } - - /** - * Constructor. - * - * Use this constructor if you want a customized wiring. - * - * @param pinRS - * pin rs - * @param pinE - * pin E - * @param pinD4 - * pin D4 - * @param pinD5 - * pin D5 - * @param pinD6 - * pin D6 - * @param pinD7 - * pin D7 - * @param pinOn - * ping 0n - */ - public Lcd20x4(GpioPin pinRS, GpioPin pinE, GpioPin pinD4, GpioPin pinD5, GpioPin pinD6, GpioPin pinD7, GpioPin pinOn) { - var pi4jRpiContext = Pi4J.newAutoContext(); - var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); - var configPinRS =createPinConfig(digitalOutputBuilder, pinRS, DigitalState.LOW); - var configPinE =createPinConfig(digitalOutputBuilder, pinE, DigitalState.LOW); - var configPinD4 =createPinConfig(digitalOutputBuilder, pinD4, DigitalState.LOW); - var configPinD5 =createPinConfig(digitalOutputBuilder, pinD5, DigitalState.LOW); - var configPinD6 =createPinConfig(digitalOutputBuilder, pinD6, DigitalState.LOW); - var configPinD7 =createPinConfig(digitalOutputBuilder, pinD7, DigitalState.LOW); - var configPinOn =createPinConfig(digitalOutputBuilder, pinOn, DigitalState.LOW); - - gpioRS = pi4jRpiContext.dout().create(configPinRS); - gpioE = pi4jRpiContext.dout().create(configPinE); - gpioD4 = pi4jRpiContext.dout().create(configPinD4); - gpioD5 = pi4jRpiContext.dout().create(configPinD5); - gpioD6 = pi4jRpiContext.dout().create(configPinD6); - gpioD7 = pi4jRpiContext.dout().create(configPinD7); - gpioOn = pi4jRpiContext.dout().create(configPinOn); + private final DigitalOutput gpioRS; + private final DigitalOutput gpioE; + private final DigitalOutput gpioD4; + private final DigitalOutput gpioD5; + private final DigitalOutput gpioD6; + private final DigitalOutput gpioD7; + private final DigitalOutput gpioOn; + + // Delay in nanos + private final static int E_DELAY = (int) TimeUnit.MICROSECONDS.toNanos(500); + + private enum Mode { + CMD(false), CHAR(true); + + private final boolean sendValue; + + Mode(boolean sendValue) { + this.sendValue = sendValue; + } + + public boolean getSendValue() { + return sendValue; + } + } + + public enum Alignment { + LEFT, CENTER, RIGHT + } + + /** + * Default constructor. + *

+ * Use this if you have used the default wiring in the example. + */ + public Lcd20x4() { + this(GpioPin.GPIO_11, GpioPin.GPIO_10, GpioPin.GPIO_06, GpioPin.GPIO_05, GpioPin.GPIO_04, GpioPin.GPIO_01, + GpioPin.GPIO_16); + } + + /** + * Constructor. + *

+ * Use this constructor if you want a customized wiring. + * + * @param pinRS pin rs + * @param pinE pin E + * @param pinD4 pin D4 + * @param pinD5 pin D5 + * @param pinD6 pin D6 + * @param pinD7 pin D7 + * @param pinOn ping 0n + */ + public Lcd20x4(GpioPin pinRS, GpioPin pinE, GpioPin pinD4, GpioPin pinD5, GpioPin pinD6, GpioPin pinD7, GpioPin pinOn) { + var pi4jRpiContext = Pi4J.newAutoContext(); + var digitalOutputBuilder = DigitalOutput.newConfigBuilder(pi4jRpiContext); + var configPinRS = createPinConfig(digitalOutputBuilder, pinRS, DigitalState.LOW); + var configPinE = createPinConfig(digitalOutputBuilder, pinE, DigitalState.LOW); + var configPinD4 = createPinConfig(digitalOutputBuilder, pinD4, DigitalState.LOW); + var configPinD5 = createPinConfig(digitalOutputBuilder, pinD5, DigitalState.LOW); + var configPinD6 = createPinConfig(digitalOutputBuilder, pinD6, DigitalState.LOW); + var configPinD7 = createPinConfig(digitalOutputBuilder, pinD7, DigitalState.LOW); + var configPinOn = createPinConfig(digitalOutputBuilder, pinOn, DigitalState.LOW); + + gpioRS = pi4jRpiContext.dout().create(configPinRS); + gpioE = pi4jRpiContext.dout().create(configPinE); + gpioD4 = pi4jRpiContext.dout().create(configPinD4); + gpioD5 = pi4jRpiContext.dout().create(configPinD5); + gpioD6 = pi4jRpiContext.dout().create(configPinD6); + gpioD7 = pi4jRpiContext.dout().create(configPinD7); + gpioOn = pi4jRpiContext.dout().create(configPinOn); // GpioController gpio = GpioFactory.getInstance(); // gpioRS = gpio.provisionDigitalOutputPin(pinRS, "RS", PinState.LOW); @@ -153,112 +149,112 @@ public Lcd20x4(GpioPin pinRS, GpioPin pinE, GpioPin pinD4, GpioPin pinD5, GpioPi // gpioD6 = gpio.provisionDigitalOutputPin(pinD6, "D6", PinState.LOW); // gpioD7 = gpio.provisionDigitalOutputPin(pinD7, "D7", PinState.LOW); // gpioOn = gpio.provisionDigitalOutputPin(pinOn, "On", PinState.HIGH); - initialize(); - } - - private DigitalOutputConfig createPinConfig(DigitalOutputConfigBuilder builder, GpioPin pin, DigitalState state){ - return builder.address(pin.address()).onState(state).build(); - } - - private void initialize() { - sendByte(0x33, Mode.CMD); - sendByte(0x32, Mode.CMD); - sendByte(0x06, Mode.CMD); - sendByte(0x0C, Mode.CMD); - sendByte(0x28, Mode.CMD); - sendByte(0x01, Mode.CMD); - sleep(E_DELAY); - } - - private void sleep(int delayNanos) { - try { - Thread.sleep(0l, delayNanos); - } catch (InterruptedException e) { - // Do not care - } - } - - private void sendByte(int bits, Mode cmd) { - System.out.println(bits); - gpioRS.setState(cmd.getSendValue()); - dataLow(); - - // Send the high bits - if ((bits & 0x10) == 0x10) { - gpioD4.setState(true); - } - - if ((bits & 0x20) == 0x20) { - gpioD5.setState(true); - } - - if ((bits & 0x40) == 0x40) { - gpioD6.setState(true); - } - - if ((bits & 0x80) == 0x80) { - gpioD7.setState(true); - } - toggleEnable(); - - // Send the low bits - dataLow(); - if ((bits & 0x01) == 0x01) { - gpioD4.setState(true); - } - - if ((bits & 0x02) == 0x02) { - gpioD5.setState(true); - } - - if ((bits & 0x04) == 0x04) { - gpioD6.setState(true); - } - - if ((bits & 0x08) == 0x08) { - gpioD7.setState(true); - } - toggleEnable(); - } - - private void dataLow() { - gpioD4.setState(false); - gpioD5.setState(false); - gpioD6.setState(false); - gpioD7.setState(false); - } - - private void toggleEnable() { - sleep(E_DELAY); - gpioE.setState(true); - sleep(E_DELAY); - gpioE.setState(false); - sleep(E_DELAY); - } - - public void sendMessage(int line, String text, Alignment alignment) { - switch (alignment) { - case RIGHT: - text = StringUtils.rightFormat(text, CHAR_WIDTH); - break; - case CENTER: - text = StringUtils.centerFormat(text, CHAR_WIDTH); - break; - default: - break; - } - sendByte(line, Mode.CMD); - for (int i = 0; i < Math.min(text.length(), CHAR_WIDTH); i++) { - sendByte(text.charAt(i), Mode.CHAR); - } - - } - - public void enableBacklight(boolean enable) { - gpioOn.setState(enable); - } - - public void clearDisplay() { - sendByte(0x01, Mode.CMD); - } + initialize(); + } + + private DigitalOutputConfig createPinConfig(DigitalOutputConfigBuilder builder, GpioPin pin, DigitalState state) { + return builder.address(pin.address()).onState(state).build(); + } + + private void initialize() { + sendByte(0x33, Mode.CMD); + sendByte(0x32, Mode.CMD); + sendByte(0x06, Mode.CMD); + sendByte(0x0C, Mode.CMD); + sendByte(0x28, Mode.CMD); + sendByte(0x01, Mode.CMD); + sleep(E_DELAY); + } + + private void sleep(int delayNanos) { + try { + Thread.sleep(0l, delayNanos); + } catch (InterruptedException e) { + // Do not care + } + } + + private void sendByte(int bits, Mode cmd) { + LOGGER.debug("sendBits:{}", bits); + gpioRS.setState(cmd.getSendValue()); + dataLow(); + + // Send the high bits + if ((bits & 0x10) == 0x10) { + gpioD4.setState(true); + } + + if ((bits & 0x20) == 0x20) { + gpioD5.setState(true); + } + + if ((bits & 0x40) == 0x40) { + gpioD6.setState(true); + } + + if ((bits & 0x80) == 0x80) { + gpioD7.setState(true); + } + toggleEnable(); + + // Send the low bits + dataLow(); + if ((bits & 0x01) == 0x01) { + gpioD4.setState(true); + } + + if ((bits & 0x02) == 0x02) { + gpioD5.setState(true); + } + + if ((bits & 0x04) == 0x04) { + gpioD6.setState(true); + } + + if ((bits & 0x08) == 0x08) { + gpioD7.setState(true); + } + toggleEnable(); + } + + private void dataLow() { + gpioD4.setState(false); + gpioD5.setState(false); + gpioD6.setState(false); + gpioD7.setState(false); + } + + private void toggleEnable() { + sleep(E_DELAY); + gpioE.setState(true); + sleep(E_DELAY); + gpioE.setState(false); + sleep(E_DELAY); + } + + public void sendMessage(int line, String text, Alignment alignment) { + switch (alignment) { + case RIGHT: + text = StringUtils.rightFormat(text, CHAR_WIDTH); + break; + case CENTER: + text = StringUtils.centerFormat(text, CHAR_WIDTH); + break; + default: + break; + } + sendByte(line, Mode.CMD); + for (int i = 0; i < Math.min(text.length(), CHAR_WIDTH); i++) { + sendByte(text.charAt(i), Mode.CHAR); + } + + } + + public void enableBacklight(boolean enable) { + gpioOn.setState(enable); + } + + public void clearDisplay() { + sendByte(0x01, Mode.CMD); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java index 3abeaf37..33f77d8f 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java @@ -16,32 +16,36 @@ */ package com.robo4j.hw.rpi.lcd; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + /** * Some simple string utils. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class StringUtils { + private static final Logger LOGGER = LoggerFactory.getLogger(StringUtils.class); + + public static String rightFormat(String string, int length) { + return String.format("%" + length + "s", string); + } - public static String rightFormat(String string, int length) { - return String.format("%" + length + "s", string); - } + public static String centerFormat(String string, int length) { + int diff = length - string.length(); + return String.format("%-" + length + "s", getSpaces(diff / 2) + string); + } - public static String centerFormat(String string, int length) { - int diff = length - string.length(); - return String.format("%-" + length + "s", getSpaces(diff / 2) + string); - } - - private static String getSpaces(int total) { - String tmp = ""; - for (int i = 0; i < total; i++) { - tmp +=" "; - } - return tmp; - } + private static String getSpaces(int total) { + String tmp = ""; + for (int i = 0; i < total; i++) { + tmp += " "; + } + return tmp; + } - public static void main(String ... bla) { - System.out.println(centerFormat("Center me", 20)); - } + public static void main(String... bla) { + LOGGER.info(centerFormat("Center me", 20)); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/VehiclePlatform.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/VehiclePlatform.java index 575d7495..60a28522 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/VehiclePlatform.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/VehiclePlatform.java @@ -17,62 +17,64 @@ package com.robo4j.hw.rpi.pwm; import com.robo4j.hw.rpi.Servo; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.util.logging.Level; -import java.util.logging.Logger; + /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class VehiclePlatform { + private static final Logger LOGGER = LoggerFactory.getLogger(VehiclePlatform.class); private final int signalEquilibrium; private final Servo throttleServo; private final Servo steeringServo; - private final Servo legServo; - private final Servo shiftServo; - private float throttle; - private float steering; - private float leg; - private float shift; + private final Servo legServo; + private final Servo shiftServo; + private float throttle; + private float steering; + private float leg; + private float shift; - public VehiclePlatform(int signalEquilibrium, Servo throttleServo, Servo steeringServo, Servo legServo, Servo shiftServo) - throws IOException { - this.signalEquilibrium = signalEquilibrium; + public VehiclePlatform(int signalEquilibrium, Servo throttleServo, Servo steeringServo, Servo legServo, Servo shiftServo) + throws IOException { + this.signalEquilibrium = signalEquilibrium; this.throttleServo = throttleServo; this.steeringServo = steeringServo; - this.legServo = legServo; - this.shiftServo = shiftServo; - resetServos(); - } + this.legServo = legServo; + this.shiftServo = shiftServo; + resetServos(); + } - private void resetServos() throws IOException { - this.throttleServo.setInput(signalEquilibrium); + private void resetServos() throws IOException { + this.throttleServo.setInput(signalEquilibrium); this.steeringServo.setInput(signalEquilibrium); this.legServo.setInput(signalEquilibrium); - this.shiftServo.setInput(signalEquilibrium); - } + this.shiftServo.setInput(signalEquilibrium); + } - public float getThrottle() throws IOException { - return throttle; - } + public float getThrottle() throws IOException { + return throttle; + } - public void setThrottle(float throttle) throws IOException { - this.throttle = throttle; - internalUpdateEngines(); - } + public void setThrottle(float throttle) throws IOException { + this.throttle = throttle; + internalUpdateEngines(); + } - public void setSteering(float steering) throws IOException { - this.steering = steering; + public void setSteering(float steering) throws IOException { + this.steering = steering; internalUpdateEngines(); - } + } - public float getSteering() { - return steering; - } + public float getSteering() { + return steering; + } public float getLeg() { return leg; @@ -91,29 +93,29 @@ public void setShift(float shift) { } private void internalUpdateEngines() { - try { - updateEngines(); - } catch (IOException e) { - Logger.getLogger(VehiclePlatform.class.getName()).log(Level.SEVERE, "Could not update engine speed!"); - } - } - - private void updateEngines() throws IOException { - processEngine(throttle, throttleServo); - processEngine(steering, steeringServo); - processEngine(leg, legServo); - processEngine(shift, shiftServo); - } - - private void processEngine(float value, Servo engine) throws IOException { - - if (value == signalEquilibrium) { - engine.setInput(signalEquilibrium); - } else if (value < signalEquilibrium) { - engine.setInput(-value); - } else { - engine.setInput(value); - } - } + try { + updateEngines(); + } catch (IOException e) { + LOGGER.error("Could not update engine speed!", e); + } + } + + private void updateEngines() throws IOException { + processEngine(throttle, throttleServo); + processEngine(steering, steeringServo); + processEngine(leg, legServo); + processEngine(shift, shiftServo); + } + + private void processEngine(float value, Servo engine) throws IOException { + + if (value == signalEquilibrium) { + engine.setInput(signalEquilibrium); + } else if (value < signalEquilibrium) { + engine.setInput(-value); + } else { + engine.setInput(value); + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTank.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTank.java index 186d03a8..afe9c81f 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTank.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pwm/roboclaw/RoboClawRCTank.java @@ -18,10 +18,10 @@ import com.robo4j.hw.rpi.Motor; import com.robo4j.hw.rpi.Servo; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.util.logging.Level; -import java.util.logging.Logger; /** * A RoboClaw engine controller, controlled with a standard servo PWM signal. @@ -29,124 +29,118 @@ * over USB. This class will only work with the RoboClaw in RC-mode, and a PWM * controller sending RC servo style signals (20 ms pulse period, 1.5 ms high * centered). It assumes mixing is not enabled. - * + *

* Note that this class is not thread safe. If changes can be initiated from * different threads, the instance must be protected. - * + * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class RoboClawRCTank implements Motor { - private final static float EPSILON = 0.05f; - private final Servo leftEngine; - private final Servo rightEngine; - private float speed; - private float direction; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboClawRCTank.class); + private final static float EPSILON = 0.05f; + private final Servo leftEngine; + private final Servo rightEngine; + private float speed; + private float direction; - /** - * Constructor. - * - * @param leftEngine - * the servo controlling the left engine. - * @param rightEngine - * the servo controlling the right engine. - * - * @throws IOException - * exception - */ - public RoboClawRCTank(Servo leftEngine, Servo rightEngine) throws IOException { - this.leftEngine = leftEngine; - this.rightEngine = rightEngine; - resetServos(); - } + /** + * Constructor. + * + * @param leftEngine the servo controlling the left engine. + * @param rightEngine the servo controlling the right engine. + * @throws IOException exception + */ + public RoboClawRCTank(Servo leftEngine, Servo rightEngine) throws IOException { + this.leftEngine = leftEngine; + this.rightEngine = rightEngine; + resetServos(); + } - private void resetServos() throws IOException { - this.leftEngine.setInput(0); - this.rightEngine.setInput(0); - } + private void resetServos() throws IOException { + this.leftEngine.setInput(0); + this.rightEngine.setInput(0); + } - @Override - public void setSpeed(float speed) throws IOException { - this.speed = speed; - internalUpdateEngines(); - } + @Override + public void setSpeed(float speed) throws IOException { + this.speed = speed; + internalUpdateEngines(); + } - /** - * @return tank engine direction in radians - */ - public float getDirection() { - return direction; - } + /** + * @return tank engine direction in radians + */ + public float getDirection() { + return direction; + } - /** - * Sets the "direction" for the platform. This directly corresponds to the - * velocity distribution between the engines. - * - * @param direction - * the desired "direction" in radians. Math.PI means all velocity - * goes to the left engine, turning the vehicle max right. - * - * @throws IOException - * exception - */ - public void setDirection(float direction) throws IOException { - this.direction = direction; - if (speed != 0) { - internalUpdateEngines(); - } - } + /** + * Sets the "direction" for the platform. This directly corresponds to the + * velocity distribution between the engines. + * + * @param direction the desired "direction" in radians. Math.PI means all velocity + * goes to the left engine, turning the vehicle max right. + * @throws IOException exception + */ + public void setDirection(float direction) throws IOException { + this.direction = direction; + if (speed != 0) { + internalUpdateEngines(); + } + } - private void updateEngines() throws IOException { - // If a component is rather small, just interpret is as zero - double rightComponent = round(Math.sin(direction)); - double forwardComponent = round(Math.cos(direction)); + private void updateEngines() throws IOException { + // If a component is rather small, just interpret is as zero + double rightComponent = round(Math.sin(direction)); + double forwardComponent = round(Math.cos(direction)); - if (speed == 0) { - leftEngine.setInput(0); - rightEngine.setInput(0); - } else if (Math.abs(forwardComponent) < EPSILON) { - // If we are trying to move, but the speed component - // forwards/backwards is very small (trying to turn a lot), then we - // turn tank style - i.e. move the wheels in opposite directions. - if (rightComponent > 0) { - rightEngine.setInput(-speed / 2); - leftEngine.setInput(speed / 2); - } else { - rightEngine.setInput(speed / 2); - leftEngine.setInput(-speed / 2); - } - } else { - float forward = 1; - if (forwardComponent < 0) { - forward = -1; - } - leftEngine.setInput(forward * (float) (speed + rightComponent * speed)); - rightEngine.setInput(forward * (float) (speed - rightComponent * speed)); - } - emitEngineEvent(); - } + if (speed == 0) { + leftEngine.setInput(0); + rightEngine.setInput(0); + } else if (Math.abs(forwardComponent) < EPSILON) { + // If we are trying to move, but the speed component + // forwards/backwards is very small (trying to turn a lot), then we + // turn tank style - i.e. move the wheels in opposite directions. + if (rightComponent > 0) { + rightEngine.setInput(-speed / 2); + leftEngine.setInput(speed / 2); + } else { + rightEngine.setInput(speed / 2); + leftEngine.setInput(-speed / 2); + } + } else { + float forward = 1; + if (forwardComponent < 0) { + forward = -1; + } + leftEngine.setInput(forward * (float) (speed + rightComponent * speed)); + rightEngine.setInput(forward * (float) (speed - rightComponent * speed)); + } + emitEngineEvent(); + } - private void emitEngineEvent() { - EngineEvent event = new EngineEvent(); - event.setDirection(direction); - event.setSpeed(speed); - event.commit(); - } + private void emitEngineEvent() { + EngineEvent event = new EngineEvent(); + event.setDirection(direction); + event.setSpeed(speed); + event.commit(); + } - private void internalUpdateEngines() { - try { - updateEngines(); - } catch (IOException e) { - Logger.getLogger(RoboClawRCTank.class.getName()).log(Level.SEVERE, "Could not update engine speed!"); - } - } + private void internalUpdateEngines() { + try { + updateEngines(); + } catch (IOException e) { + LOGGER.error("Could not update engine speed!", e); + } + } - private double round(double val) { - return Math.abs(val) < EPSILON ? 0 : val; - } + private double round(double val) { + return Math.abs(val) < EPSILON ? 0 : val; + } - @Override - public float getSpeed() throws IOException { - return leftEngine.getInput(); - } + @Override + public float getSpeed() throws IOException { + return leftEngine.getInput(); + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java index 917cf82f..cccba4a0 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java @@ -16,6 +16,10 @@ */ package com.robo4j.hw.rpi.serial; +import com.pi4j.io.serial.Serial; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.io.IOException; import java.nio.ByteBuffer; import java.util.HashMap; @@ -23,128 +27,119 @@ import java.util.Map; import java.util.Set; import java.util.concurrent.TimeoutException; -import java.util.logging.Logger; - -import com.pi4j.io.serial.Serial; //import com.pi4j.util.ExecUtil; public class SerialUtil { + private static final Logger LOGGER = LoggerFactory.getLogger(SerialUtil.class); - // TODO, alternative to ExecUtil - public static Set getAvailableUSBSerialDevices() throws IOException, InterruptedException { - Set descriptors = new HashSet<>(); + // TODO, alternative to ExecUtil + public static Set getAvailableUSBSerialDevices() throws IOException, InterruptedException { + Set descriptors = new HashSet<>(); // String[] devices = ExecUtil.execute("ls /sys/bus/usb-serial/devices/"); // for (String device : devices) { // Map metadata = asMetadata(ExecUtil.execute("cat /sys/bus/usb-serial/devices/" + device + "/../uevent")); // String path = "/dev/" + device; // descriptors.add(createSerialDescriptor(path, metadata)); // } - return descriptors; - } + return descriptors; + } - /** - * Will attempt to read to read the indicated number of bytes from the - * serial port. - * - * @param serial - * the (opened) serial port to read from. - * @param timeout - * the timeout after which to fail. - * @return the byte [] with the result. - * @throws IOException - * @throws IllegalStateException - * @throws InterruptedException - */ - public static byte[] readBytes(Serial serial, int bytes, long timeout) - throws IllegalStateException, IOException, InterruptedException, TimeoutException { - ByteBuffer buffer = ByteBuffer.allocate(bytes); - readBytes(buffer, serial, bytes, timeout); - return buffer.array(); - } + /** + * Will attempt to read to read the indicated number of bytes from the + * serial port. + * + * @param serial the (opened) serial port to read from. + * @param timeout the timeout after which to fail. + * @return the byte [] with the result. + * @throws IOException exception + * @throws IllegalStateException illegal exception + * @throws InterruptedException interrupted exception + */ + public static byte[] readBytes(Serial serial, int bytes, long timeout) + throws IllegalStateException, InterruptedException, TimeoutException { + ByteBuffer buffer = ByteBuffer.allocate(bytes); + readBytes(buffer, serial, bytes, timeout); + return buffer.array(); + } - /** - * Will attempt to read to read the indicated number of bytes from the - * serial port into the provided buffer. It's up to you to ensure that there - * is enough space to hold the data. - * - * @param buffer - * the byte buffer to read into. - * @param serial - * the (opened) serial port to read from. - * @param bytes - * the number of bytes to read. - * @param timeout - * the timeout after which to fail. - * @throws IOException - * @throws IllegalStateException - * @throws InterruptedException - */ - public static void readBytes(ByteBuffer buffer, Serial serial, int bytes, long timeout) - throws IllegalStateException, IOException, InterruptedException, TimeoutException { - int available = serial.available(); + /** + * Will attempt to read to read the indicated number of bytes from the + * serial port into the provided buffer. It's up to you to ensure that there + * is enough space to hold the data. + * + * @param buffer the byte buffer to read into. + * @param serial the (opened) serial port to read from. + * @param bytes the number of bytes to read. + * @param timeout the timeout after which to fail. + * @throws IllegalStateException + * @throws InterruptedException + */ + public static void readBytes(ByteBuffer buffer, Serial serial, int bytes, long timeout) + throws IllegalStateException, InterruptedException, TimeoutException { + int available = serial.available(); - if (available >= bytes) { - serial.read(buffer, bytes); - return; - } + if (available >= bytes) { + serial.read(buffer, bytes); + return; + } - long startTime = System.currentTimeMillis(); - int leftToRead = bytes; - while (leftToRead > 0) { - if (System.currentTimeMillis() - startTime > timeout) { - throw new TimeoutException("Could not read bytes in time"); - } - available = serial.available(); - if (available == 0) { - Thread.sleep(40); - continue; - } - int nextRead = Math.min(leftToRead, available); - serial.read(buffer, nextRead); - leftToRead -= nextRead; - } - } + long startTime = System.currentTimeMillis(); + int leftToRead = bytes; + while (leftToRead > 0) { + if (System.currentTimeMillis() - startTime > timeout) { + throw new TimeoutException("Could not read bytes in time"); + } + available = serial.available(); + if (available == 0) { + Thread.sleep(40); + continue; + } + int nextRead = Math.min(leftToRead, available); + serial.read(buffer, nextRead); + leftToRead -= nextRead; + } + } - private static Map asMetadata(String[] lines) { - Map metadata = new HashMap<>(); - for (String line : lines) { - String[] lineData = line.split("="); - if (lineData.length == 2) { - metadata.put(lineData[0], lineData[1]); - } else { - Logger.getLogger(SerialUtil.class.getName()).warning("Failed to parse usb serial metadata: " + line); - } - } - return metadata; - } + private static Map asMetadata(String[] lines) { + Map metadata = new HashMap<>(); + for (String line : lines) { + String[] lineData = line.split("="); + if (lineData.length == 2) { + metadata.put(lineData[0], lineData[1]); + } else { + LOGGER.warn("Failed to parse usb serial metadata: {}", line); + } + } + return metadata; + } - private static SerialDeviceDescriptor createSerialDescriptor(String path, Map metadata) { - String productString = metadata.get("PRODUCT"); - if (productString != null) { - String[] productStrings = productString.split("/"); - if (productStrings.length >= 2) { - return new SerialDeviceDescriptor(path, productStrings[0], productStrings[1]); - } - } - return new SerialDeviceDescriptor(path, null, null); - } + private static SerialDeviceDescriptor createSerialDescriptor(String path, Map metadata) { + String productString = metadata.get("PRODUCT"); + if (productString != null) { + String[] productStrings = productString.split("/"); + if (productStrings.length >= 2) { + return new SerialDeviceDescriptor(path, productStrings[0], productStrings[1]); + } + } + return new SerialDeviceDescriptor(path, null, null); + } - /** - * Prints all the available USB serial devices found. - * - * @throws InterruptedException - * @throws IOException - */ - public static void main(String[] args) throws IOException, InterruptedException { - Set descriptors = getAvailableUSBSerialDevices(); - if (descriptors.isEmpty()) { - System.out.println("No usb serial devices found!"); - } else { - System.out.println("Printing usb serial devices found:"); - for (SerialDeviceDescriptor descriptor : descriptors) { - System.out.println(descriptor); - } - System.out.println("Done!"); - } - } + /** + * Prints all the available USB serial devices found. + * + * @throws InterruptedException interrupted exception + * @throws IOException io exception + */ + public static void main(String[] args) throws IOException, InterruptedException { + Set descriptors = getAvailableUSBSerialDevices(); + if (descriptors.isEmpty()) { + LOGGER.info("No usb serial devices found!"); + } else { + LOGGER.info("Printing usb serial devices found:"); + for (SerialDeviceDescriptor descriptor : descriptors) { + LOGGER.info("descriptor:{}", descriptor); + } + LOGGER.info("Done!"); + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java index 86cd3aff..9fdf81bd 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java @@ -16,24 +16,20 @@ */ package com.robo4j.hw.rpi.serial.gps; -import java.io.IOException; -import java.nio.charset.StandardCharsets; -import java.util.List; -import java.util.StringTokenizer; -import java.util.concurrent.CopyOnWriteArrayList; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; -import java.util.logging.Level; -import java.util.logging.Logger; - import com.pi4j.Pi4J; import com.pi4j.io.serial.Baud; import com.pi4j.io.serial.Serial; import com.robo4j.hw.rpi.gps.GPS; import com.robo4j.hw.rpi.gps.GPSListener; import com.robo4j.hw.rpi.gps.NmeaUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.nio.charset.StandardCharsets; +import java.util.List; +import java.util.StringTokenizer; +import java.util.concurrent.*; /** * Code to talk to the Adafruit "ultimate GPS" over the serial port. @@ -45,209 +41,206 @@ * @author Miro Wengner (@miragemiko) */ public class MTK3339GPS implements GPS { - /** - * The position accuracy without any - */ - public static final float UNAIDED_POSITION_ACCURACY = 3.0f; - - /** - * The default read interval to use when auto updating. - */ - public static final int DEFAULT_READ_INTERVAL = 550; - - /** - * The default serial port is /dev/serial0. Since Raspberry Pi 3 nabbed the - * /dev/ttyAMA0 for the bluetooth, serial0 should be the new logical name to - * use for the rx/tx pins. This is supposedly compatible with the older - * Raspberry Pis as well. - */ - public static final String DEFAULT_GPS_PORT = "/dev/serial0"; - - private static final String POSITION_TAG = "$GPGGA"; - private static final String VELOCITY_TAG = "$GPVTG"; - - private static final int BAUD_DEFAULT = 9600; - - private final String serialPort; - private final Serial serial; -// private final ExecutorServiceFactory serviceFactory; - private final GPSDataRetriever dataRetriever; - private final int readInterval; - - private final ScheduledExecutorService internalExecutor = Executors.newScheduledThreadPool(1, (r) -> { - Thread t = new Thread(r, "GPS Internal Executor"); - t.setDaemon(true); - return t; - - }); - private final List listeners = new CopyOnWriteArrayList(); - - // This is the scheduled future controlling the auto updates. - private ScheduledFuture scheduledFuture; - - /** - * Creates a new GPS instance. Will use an internal thread to read data, and - * the default read interval. - * - * @throws IOException - * exception - */ - public MTK3339GPS() throws IOException { - this(DEFAULT_GPS_PORT, DEFAULT_READ_INTERVAL); - } - - /** - * Creates a new GPS instance. - * - * @param serialPort - * serial port - * @param readInterval - * read internal - * - * @throws IOException - * exception - */ - public MTK3339GPS(String serialPort, int readInterval) throws IOException { - this.serialPort = serialPort; - this.readInterval = readInterval; + private static final Logger LOGGER = LoggerFactory.getLogger(MTK3339GPS.class); + + /** + * The position accuracy without any + */ + public static final float UNAIDED_POSITION_ACCURACY = 3.0f; + + /** + * The default read interval to use when auto updating. + */ + public static final int DEFAULT_READ_INTERVAL = 550; + + /** + * The default serial port is /dev/serial0. Since Raspberry Pi 3 nabbed the + * /dev/ttyAMA0 for the bluetooth, serial0 should be the new logical name to + * use for the rx/tx pins. This is supposedly compatible with the older + * Raspberry Pis as well. + */ + public static final String DEFAULT_GPS_PORT = "/dev/serial0"; + + private static final String POSITION_TAG = "$GPGGA"; + private static final String VELOCITY_TAG = "$GPVTG"; + + private static final int BAUD_DEFAULT = 9600; + + private final String serialPort; + private final Serial serial; + // private final ExecutorServiceFactory serviceFactory; + private final GPSDataRetriever dataRetriever; + private final int readInterval; + + private final ScheduledExecutorService internalExecutor = Executors.newScheduledThreadPool(1, (r) -> { + Thread t = new Thread(r, "GPS Internal Executor"); + t.setDaemon(true); + return t; + + }); + private final List listeners = new CopyOnWriteArrayList(); + + // This is the scheduled future controlling the auto updates. + private ScheduledFuture scheduledFuture; + + /** + * Creates a new GPS instance. Will use an internal thread to read data, and + * the default read interval. + * + * @throws IOException exception + */ + public MTK3339GPS() throws IOException { + this(DEFAULT_GPS_PORT, DEFAULT_READ_INTERVAL); + } + + /** + * Creates a new GPS instance. + * + * @param serialPort serial port + * @param readInterval read internal + * @throws IOException exception + */ + public MTK3339GPS(String serialPort, int readInterval) throws IOException { + this.serialPort = serialPort; + this.readInterval = readInterval; // serial = SerialFactory.createInstance(); // serviceFactory = SerialFactory.getExecutorServiceFactory(); - var pi4jRpiContext = Pi4J.newAutoContext(); - this.serial = pi4jRpiContext.create(Serial.newConfigBuilder(pi4jRpiContext) - .id(serialPort) - //TODO review default baud - .baud(Baud._9600).build()); - - dataRetriever = new GPSDataRetriever(); - initialize(); - } - - @Override - public void addListener(GPSListener gpsListener) { - listeners.add(gpsListener); - } - - @Override - public void removeListener(GPSListener gpsListener) { - listeners.remove(gpsListener); - } - - /** - * Shuts down the GPS listener. After the shutdown is completed, no more - * events will be sent to listeners. - */ - public void shutdown() { - synchronized (internalExecutor) { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - awaitTermination(); - } - try { - serial.close(); - // // FIXME(Marcus/Jul 30, 2017): This is kinda scary to do in a - // component! - // TODO review + var pi4jRpiContext = Pi4J.newAutoContext(); + this.serial = pi4jRpiContext.create(Serial.newConfigBuilder(pi4jRpiContext) + .id(serialPort) + //TODO review default baud + .baud(Baud._9600).build()); + + dataRetriever = new GPSDataRetriever(); + initialize(); + } + + @Override + public void addListener(GPSListener gpsListener) { + listeners.add(gpsListener); + } + + @Override + public void removeListener(GPSListener gpsListener) { + listeners.remove(gpsListener); + } + + /** + * Shuts down the GPS listener. After the shutdown is completed, no more + * events will be sent to listeners. + */ + public void shutdown() { + synchronized (internalExecutor) { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + awaitTermination(); + } + try { + serial.close(); + // // FIXME(Marcus/Jul 30, 2017): This is kinda scary to do in a + // component! + // TODO review // serviceFactory.shutdown(); - } catch (IllegalStateException e) { - // Don't care, we're shutting down. - } - } - - private void awaitTermination() { - try { - internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS); - internalExecutor.shutdown(); - } catch (InterruptedException e) { - // Don't care if we were interrupted. - } - } - - /** - * Call this to perform a read of data from the device in the current - * thread. Use this for scheduling reads yourself. - */ - public void update() { - dataRetriever.update(); - } - - /** - * Call this to start reading automatically, using an internal scheduler. - */ - public void start() { - synchronized (internalExecutor) { - if (scheduledFuture == null) { - scheduledFuture = internalExecutor.scheduleAtFixedRate(dataRetriever, 0, readInterval, TimeUnit.MILLISECONDS); - } - throw new IllegalStateException("Auto update already started!"); - } - } - - private void initialize() throws IOException { - serial.open(); + } catch (IllegalStateException e) { + // Don't care, we're shutting down. + } + } + + private void awaitTermination() { + try { + internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS); + internalExecutor.shutdown(); + } catch (InterruptedException e) { + // Don't care if we were interrupted. + } + } + + /** + * Call this to perform a read of data from the device in the current + * thread. Use this for scheduling reads yourself. + */ + public void update() { + dataRetriever.update(); + } + + /** + * Call this to start reading automatically, using an internal scheduler. + */ + public void start() { + synchronized (internalExecutor) { + if (scheduledFuture == null) { + scheduledFuture = internalExecutor.scheduleAtFixedRate(dataRetriever, 0, readInterval, TimeUnit.MILLISECONDS); + } + throw new IllegalStateException("Auto update already started!"); + } + } + + private void initialize() throws IOException { + serial.open(); // serial.open(serialPort, BAUD_DEFAULT); - } - - private void notifyListeners(MTK3339PositionEvent event) { - for (GPSListener listener : listeners) { - listener.onPosition(event); - } - } - - private void notifyListeners(MTK3339VelocityEvent event) { - for (GPSListener listener : listeners) { - listener.onVelocity(event); - } - } - - private final class GPSDataRetriever implements Runnable { - private final StringBuilder builder = new StringBuilder(); - - @Override - public void run() { - update(); - } - - public void update() { - String str = ""; - try { - str = readNext(builder); - } catch (IllegalStateException | IOException e) { - Logger.getLogger(MTK3339GPS.class.getName()).log(Level.WARNING, "Error reading line", e); - } - builder.setLength(0); - StringTokenizer st = new StringTokenizer(str, "\n", true); - while (st.hasMoreElements()) { - String dataLine = st.nextToken(); - while ("\n".equals(dataLine) && st.hasMoreElements()) { - dataLine = st.nextToken(); - } - if (st.hasMoreElements()) { - consume(dataLine); - } else if (!"\n".equals(dataLine)) { - builder.append(dataLine); - } - } - } - - private void consume(String dataLine) { - if (dataLine.startsWith("$")) { - if (dataLine.startsWith(POSITION_TAG) && NmeaUtils.hasValidCheckSum(dataLine)) { - notifyListeners(new MTK3339PositionEvent(MTK3339GPS.this, dataLine)); - } else if (dataLine.startsWith(VELOCITY_TAG) && NmeaUtils.hasValidCheckSum(dataLine)) { - notifyListeners(new MTK3339VelocityEvent(MTK3339GPS.this, dataLine)); - } - } - } - - private String readNext(StringBuilder builder) throws IllegalStateException, IOException { + } + + private void notifyListeners(MTK3339PositionEvent event) { + for (GPSListener listener : listeners) { + listener.onPosition(event); + } + } + + private void notifyListeners(MTK3339VelocityEvent event) { + for (GPSListener listener : listeners) { + listener.onVelocity(event); + } + } + + private final class GPSDataRetriever implements Runnable { + private final StringBuilder builder = new StringBuilder(); + + @Override + public void run() { + update(); + } + + public void update() { + String str = ""; + try { + str = readNext(builder); + } catch (IllegalStateException | IOException e) { + LOGGER.error("Error reading line, update", e); + } + builder.setLength(0); + StringTokenizer st = new StringTokenizer(str, "\n", true); + while (st.hasMoreElements()) { + String dataLine = st.nextToken(); + while ("\n".equals(dataLine) && st.hasMoreElements()) { + dataLine = st.nextToken(); + } + if (st.hasMoreElements()) { + consume(dataLine); + } else if (!"\n".equals(dataLine)) { + builder.append(dataLine); + } + } + } + + private void consume(String dataLine) { + if (dataLine.startsWith("$")) { + if (dataLine.startsWith(POSITION_TAG) && NmeaUtils.hasValidCheckSum(dataLine)) { + notifyListeners(new MTK3339PositionEvent(MTK3339GPS.this, dataLine)); + } else if (dataLine.startsWith(VELOCITY_TAG) && NmeaUtils.hasValidCheckSum(dataLine)) { + notifyListeners(new MTK3339VelocityEvent(MTK3339GPS.this, dataLine)); + } + } + } + + private String readNext(StringBuilder builder) throws IllegalStateException, IOException { // builder.append(new String(serial.read(), StandardCharsets.US_ASCII)); - // TODO : improve - var buffer = new byte[serial.available()]; - serial.read(buffer); - builder.append(new String(buffer, StandardCharsets.US_ASCII)); - return builder.toString(); - } - } + // TODO : improve + var buffer = new byte[serial.available()]; + serial.read(buffer); + builder.append(new String(buffer, StandardCharsets.US_ASCII)); + return builder.toString(); + } + } } diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java index 45bb65f0..49a19863 100644 --- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java +++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java @@ -28,12 +28,9 @@ import com.robo4j.math.geometry.impl.ScanResultImpl; import com.robo4j.math.jfr.ScanEvent; import com.robo4j.math.jfr.ScanId; -import jdk.jfr.Category; -import jdk.jfr.Description; -import jdk.jfr.Event; -import jdk.jfr.Label; -import jdk.jfr.Name; -import jdk.jfr.StackTrace; +import jdk.jfr.*; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.nio.ByteBuffer; @@ -42,8 +39,6 @@ import java.util.List; import java.util.Set; import java.util.concurrent.TimeoutException; -import java.util.logging.Level; -import java.util.logging.Logger; /** * Driver for the ydlidar device. @@ -69,7 +64,7 @@ * @author Miro Wengner (@miragemiko) */ public class YDLidarDevice { - private static final Logger LOGGER = Logger.getLogger(YDLidarDevice.class.getName()); + private static final Logger LOGGER = LoggerFactory.getLogger(YDLidarDevice.class); private static final int DEFAULT_SERIAL_TIMEOUT = 800; public static final String SERIAL_PORT_AUTO = "auto"; @@ -200,7 +195,7 @@ public void run() { try { DataHeader header = readDataHeader(DEFAULT_SERIAL_TIMEOUT * 4); if (!header.isValid()) { - LOGGER.log(Level.SEVERE, "Got invalid header - stopping scanner"); + LOGGER.warn("Got invalid header - stopping scanner"); stopScanning(); return; } @@ -216,7 +211,7 @@ public void run() { // If we aren't scanning, we got here since we are // shutting down scanning, and there is not much // left to do. - LOGGER.log(Level.SEVERE, "Failed to read data from the ydlidar - stopping scanner", e); + LOGGER.error("Failed to read data from the ydlidar - stopping scanner", e); stopScanning(); } return; @@ -487,7 +482,7 @@ public void shutdown() { disableDataCapturing(); serial.close(); } catch (IllegalStateException | IOException | InterruptedException e) { - LOGGER.log(Level.WARNING, "Problem shutting down ydlidar serial", e); + LOGGER.error("Problem shutting down ydlidar serial", e); } } } @@ -590,7 +585,7 @@ private static String autoResolveSerialPort() throws IOException, InterruptedExc Set availableUSBSerialDevices = SerialUtil.getAvailableUSBSerialDevices(); for (SerialDeviceDescriptor descriptor : availableUSBSerialDevices) { if (VENDOR_ID.equals(descriptor.getVendorId()) && PRODUCT_ID.equals(descriptor.getProductId())) { - LOGGER.info("Bound ydlidar to " + descriptor); + LOGGER.info("Bound ydlidar to {}", descriptor); return descriptor.getPath(); } } diff --git a/robo4j-hw-rpi/src/main/java/module-info.java b/robo4j-hw-rpi/src/main/java/module-info.java index 9636727d..26bd4c2a 100644 --- a/robo4j-hw-rpi/src/main/java/module-info.java +++ b/robo4j-hw-rpi/src/main/java/module-info.java @@ -9,6 +9,7 @@ requires com.pi4j; requires com.pi4j.plugin.raspberrypi; requires com.pi4j.plugin.linuxfs; + requires org.slf4j; exports com.robo4j.hw.rpi; exports com.robo4j.hw.rpi.camera; diff --git a/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestEventDecoding.java b/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestEventDecoding.java index 3e3af33a..bf094b05 100644 --- a/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestEventDecoding.java +++ b/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestEventDecoding.java @@ -16,48 +16,45 @@ */ package com.robo4j.hw.rpi.i2c.gps; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertSame; -import static org.junit.jupiter.api.Assertions.assertTrue; - +import com.robo4j.hw.rpi.gps.*; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import com.robo4j.hw.rpi.gps.GPS; -import com.robo4j.hw.rpi.gps.MockGPS; -import com.robo4j.hw.rpi.gps.NmeaUtils; -import com.robo4j.hw.rpi.gps.PositionEvent; -import com.robo4j.hw.rpi.gps.VelocityEvent; +import static org.junit.jupiter.api.Assertions.*; public class TestEventDecoding { - @Test - void testDecodePositionString() { - GPS gps = new MockGPS(); - String line = "$GNGGA,223249.000,4704.833492,N,00826.465532,E,1,12,0.87,445.503,M,48.002,M,,*70"; - assertTrue(NmeaUtils.hasValidCheckSum(line), "Not a valid checksum!"); - - PositionEvent event = XA1110PositionEvent.decode(gps, line); - - assertSame(gps, event.getSource()); - assertNotNull(event.getSource()); - assertNotNull(event); - assertNotNull(event.getLocation()); - - System.out.println(event); - } - - @Test - void testDecodeVelocityString() { - GPS gps = new MockGPS(); - String line = "$GNVTG,343.70,T,,M,0.70,N,1.30,K,A*25"; - assertTrue(NmeaUtils.hasValidCheckSum(line), "Not a valid checksum!"); - - VelocityEvent event = XA1110VelocityEvent.decode(gps, line); - - assertSame(gps, event.getSource()); - assertNotNull(event.getSource()); - assertNotNull(event); - assertNotNull(event.getGroundSpeed()); - - System.out.println(event); - } + private static final Logger LOGGER = LoggerFactory.getLogger(TestEventDecoding.class); + + @Test + void testDecodePositionString() { + GPS gps = new MockGPS(); + String line = "$GNGGA,223249.000,4704.833492,N,00826.465532,E,1,12,0.87,445.503,M,48.002,M,,*70"; + + PositionEvent event = XA1110PositionEvent.decode(gps, line); + + LOGGER.info(event.toString()); + + assertTrue(NmeaUtils.hasValidCheckSum(line), "Not a valid checksum!"); + assertSame(gps, event.getSource()); + assertNotNull(event.getSource()); + assertNotNull(event); + assertNotNull(event.getLocation()); + } + + @Test + void testDecodeVelocityString() { + GPS gps = new MockGPS(); + String line = "$GNVTG,343.70,T,,M,0.70,N,1.30,K,A*25"; + assertTrue(NmeaUtils.hasValidCheckSum(line), "Not a valid checksum!"); + + VelocityEvent event = XA1110VelocityEvent.decode(gps, line); + + LOGGER.info("event: {}", event); + + assertSame(gps, event.getSource()); + assertNotNull(event.getSource()); + assertNotNull(event); + event.getGroundSpeed(); + } } diff --git a/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestGPS.java b/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestGPS.java index 4105ad6b..f5a41fb2 100644 --- a/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestGPS.java +++ b/robo4j-hw-rpi/src/test/java/com/robo4j/hw/rpi/i2c/gps/TestGPS.java @@ -16,34 +16,36 @@ */ package com.robo4j.hw.rpi.i2c.gps; -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; - import com.robo4j.hw.rpi.i2c.gps.XA1110Device.NmeaSentenceType; import com.robo4j.hw.rpi.i2c.gps.XA1110Device.NmeaSetting; +import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import static org.junit.jupiter.api.Assertions.assertEquals; public class TestGPS { + private static final Logger LOGGER = LoggerFactory.getLogger(TestGPS.class); - @Test - public void testCreateMTKCommand() { - // Also tests the checksum calculation - example from the documentation. - String mtkPacket = XA1110Device.createMtkPacket(XA1110Device.PacketType.TEST, null); - assertEquals("$PMTK000*32\r\n", mtkPacket); - } + @Test + public void testCreateMTKCommand() { + // Also tests the checksum calculation - example from the documentation. + String mtkPacket = XA1110Device.createMtkPacket(XA1110Device.PacketType.TEST, null); + assertEquals("$PMTK000*32\r\n", mtkPacket); + } - @Test - public void testCreateNmeaSetupPacket() { - // Tests that it is possible to create a command packet for controlling - // what Nmea packets to receive and at what frequency. Uses example from - // the documentation, but corrected, since it was wrong... - String mtkPacket = XA1110Device.createNmeaSentencesAndFrequenciesMtkPacket(new NmeaSetting(NmeaSentenceType.GEOPOS, 1), - new NmeaSetting(NmeaSentenceType.RECOMMENDED_MIN_SPEC, 1), new NmeaSetting(NmeaSentenceType.COURSE_AND_SPEED, 1), - new NmeaSetting(NmeaSentenceType.FIX_DATA, 1), new NmeaSetting(NmeaSentenceType.DOPS_SAT, 1), - new NmeaSetting(NmeaSentenceType.SATS_IN_VIEW, 5), new NmeaSetting(NmeaSentenceType.MTK_DEBUG, 1), - new NmeaSetting(NmeaSentenceType.TIME_DATE, 1)); + @Test + public void testCreateNmeaSetupPacket() { + // Tests that it is possible to create a command packet for controlling + // what Nmea packets to receive and at what frequency. Uses example from + // the documentation, but corrected, since it was wrong... + String mtkPacket = XA1110Device.createNmeaSentencesAndFrequenciesMtkPacket(new NmeaSetting(NmeaSentenceType.GEOPOS, 1), + new NmeaSetting(NmeaSentenceType.RECOMMENDED_MIN_SPEC, 1), new NmeaSetting(NmeaSentenceType.COURSE_AND_SPEED, 1), + new NmeaSetting(NmeaSentenceType.FIX_DATA, 1), new NmeaSetting(NmeaSentenceType.DOPS_SAT, 1), + new NmeaSetting(NmeaSentenceType.SATS_IN_VIEW, 5), new NmeaSetting(NmeaSentenceType.MTK_DEBUG, 1), + new NmeaSetting(NmeaSentenceType.TIME_DATE, 1)); - System.out.println(mtkPacket); - assertEquals("$PMTK314,1,1,1,1,1,5,0,0,0,0,0,0,0,0,0,0,1,1,0*2c\r\n", mtkPacket); - } + LOGGER.debug(mtkPacket); + assertEquals("$PMTK314,1,1,1,1,1,5,0,0,0,0,0,0,0,0,0,0,1,1,0*2c\r\n", mtkPacket); + } } From e4e7860d369774a89e8a05ef2b4e5e14b4ce32ae Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Mon, 7 Oct 2024 22:01:26 +0200 Subject: [PATCH 04/13] [69] robo4j-math logger update --- robo4j-math/pom.xml | 4 + .../math/features/FeatureExtraction.java | 563 +++++++++--------- .../com/robo4j/math/geometry/MatrixEmpty.java | 5 +- robo4j-math/src/main/java/module-info.java | 1 + .../com/robo4j/math/geometry/MatrixTest.java | 311 +++++----- 5 files changed, 448 insertions(+), 436 deletions(-) diff --git a/robo4j-math/pom.xml b/robo4j-math/pom.xml index 21518181..6c06e1c2 100644 --- a/robo4j-math/pom.xml +++ b/robo4j-math/pom.xml @@ -42,5 +42,9 @@ junit-jupiter-params test + + org.slf4j + slf4j-api + \ No newline at end of file diff --git a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java index cc83e4ae..d2c5851a 100644 --- a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java +++ b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java @@ -16,299 +16,300 @@ */ package com.robo4j.math.features; +import com.robo4j.math.geometry.CurvaturePoint2f; +import com.robo4j.math.geometry.Line2f; +import com.robo4j.math.geometry.Point2f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.ArrayList; import java.util.Collection; import java.util.Iterator; import java.util.List; -import com.robo4j.math.geometry.CurvaturePoint2f; -import com.robo4j.math.geometry.Line2f; -import com.robo4j.math.geometry.Point2f; - /** * Simple and fast feature extraction from lists of Point2f. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ // TODO review usage public class FeatureExtraction { - /** - * The residual variance of the Lidar Lite (25 mm). - */ - private static final double RESIDUAL_VARIANCE = 0.025f; - - /** - * Auxiliary constant parameter for segmenting - */ - private static final double AUXILIARY_CONSTANT = Math.toRadians(10); - - /** - * Length variance constant - */ - private static final double Uk = 0.02; - - /** - * Minimal samples for a Line2D - */ - private static final int MIN_Line2D_SAMPLES = 4; - - /** - * Minimal angle deviation for a Line2D - */ - private static final float Line2D_ANGLE_THRESHOLD = 0.5f; - - /** - * Minimal angle deviation to be part of a possible corner - */ - private static final float CURVATURE_THRESHOLD = (float) (Math.PI / 6); - - /** - * Minimal angle deviation to be a corner - */ - private static final float CORNER_THRESHOLD = (float) (Math.PI / 3); - - /** - * Calculates the segments in the scan, using the Borg and Aldon adaptive - * break algorithm. - * - * @param Point2fs - * the Point2fs to segment. - * @param angularResolution - * of the scan in radians. - * - * @return the Point2fs broken up into segments. - */ - public static List> segment(List Point2fs, float angularResolution) { - List> segments = new ArrayList>(10); - - Iterator iterator = Point2fs.iterator(); - List currentSegment = new ArrayList(); - segments.add(currentSegment); - - Point2f lastPoint2D = iterator.next(); - currentSegment.add(lastPoint2D); - - while (iterator.hasNext()) { - Point2f nextPoint2D = iterator.next(); - float delta = nextPoint2D.distance(lastPoint2D); - double maxRange = segmentMaxRange(lastPoint2D.getRange(), angularResolution); - if (delta > maxRange) { - currentSegment = new ArrayList(); - segments.add(currentSegment); - } - currentSegment.add(nextPoint2D); - lastPoint2D = nextPoint2D; - } - return segments; - } - - public static float calculateVectorAngle(Point2f b, Point2f center, Point2f f) { - if (b.equals(center) || f.equals(center)) { - return 0; - } - - double bDeltaX = center.getX() - b.getX(); - double bDeltaY = center.getY() - b.getY(); - - double fDeltaX = f.getX() - center.getX(); - double fDeltaY = f.getY() - center.getY(); - - return (float) (Math.atan2(fDeltaX, fDeltaY) - Math.atan2(bDeltaX, bDeltaY)); - } - - private static double segmentMaxRange(float lastRange, float angularResolution) { - return lastRange * Math.sin(angularResolution) / Math.sin(AUXILIARY_CONSTANT - angularResolution) + 3 * RESIDUAL_VARIANCE; - } - - public static float[] calculateSimpleVectorAngles(List Point2fs) { - if (Point2fs.size() < 5) { - return null; - } - - float[] alphas = new float[Point2fs.size()]; - for (int i = 0; i < Point2fs.size(); i++) { - Point2f before = i == 0 ? Point2fs.get(0) : Point2fs.get(i - 1); - Point2f center = Point2fs.get(i); - Point2f following = i == Point2fs.size() - 1 ? Point2fs.get(i) : Point2fs.get(i + 1); - alphas[i] = calculateVectorAngle(before, center, following); - } - return alphas; - } - - public static FeatureSet getFeatures(List sample, float angularResolution) { - return extractFeatures(sample, angularResolution); - } - - private static FeatureSet extractFeatures(List sample, float angularResolution) { - List> segments = segment(sample, angularResolution); - List corners = new ArrayList<>(); - List Line2fs = new ArrayList<>(); - for (List Point2fs : segments) { - if (Point2fs.size() < MIN_Line2D_SAMPLES) { - continue; - } - float[] deltaAngles = calculateSamplePoint2DDeltaAngles(Point2fs); - if (deltaAngles == null) { - continue; - } - Line2fs.addAll(extractLine2Ds(Point2fs, deltaAngles)); - corners.addAll(extractCorners(Point2fs, deltaAngles)); - } + private static final Logger LOGGER = LoggerFactory.getLogger(FeatureExtraction.class); + /** + * The residual variance of the Lidar Lite (25 mm). + */ + private static final double RESIDUAL_VARIANCE = 0.025f; + + /** + * Auxiliary constant parameter for segmenting + */ + private static final double AUXILIARY_CONSTANT = Math.toRadians(10); + + /** + * Length variance constant + */ + private static final double Uk = 0.02; + + /** + * Minimal samples for a Line2D + */ + private static final int MIN_Line2D_SAMPLES = 4; + + /** + * Minimal angle deviation for a Line2D + */ + private static final float Line2D_ANGLE_THRESHOLD = 0.5f; + + /** + * Minimal angle deviation to be part of a possible corner + */ + private static final float CURVATURE_THRESHOLD = (float) (Math.PI / 6); + + /** + * Minimal angle deviation to be a corner + */ + private static final float CORNER_THRESHOLD = (float) (Math.PI / 3); + + /** + * Calculates the segments in the scan, using the Borg and Aldon adaptive + * break algorithm. + * + * @param Point2fs the Point2fs to segment. + * @param angularResolution of the scan in radians. + * @return the Point2fs broken up into segments. + */ + public static List> segment(List Point2fs, float angularResolution) { + List> segments = new ArrayList>(10); + + Iterator iterator = Point2fs.iterator(); + List currentSegment = new ArrayList(); + segments.add(currentSegment); + + Point2f lastPoint2D = iterator.next(); + currentSegment.add(lastPoint2D); + + while (iterator.hasNext()) { + Point2f nextPoint2D = iterator.next(); + float delta = nextPoint2D.distance(lastPoint2D); + double maxRange = segmentMaxRange(lastPoint2D.getRange(), angularResolution); + if (delta > maxRange) { + currentSegment = new ArrayList(); + segments.add(currentSegment); + } + currentSegment.add(nextPoint2D); + lastPoint2D = nextPoint2D; + } + return segments; + } + + public static float calculateVectorAngle(Point2f b, Point2f center, Point2f f) { + if (b.equals(center) || f.equals(center)) { + return 0; + } + + double bDeltaX = center.getX() - b.getX(); + double bDeltaY = center.getY() - b.getY(); + + double fDeltaX = f.getX() - center.getX(); + double fDeltaY = f.getY() - center.getY(); + + return (float) (Math.atan2(fDeltaX, fDeltaY) - Math.atan2(bDeltaX, bDeltaY)); + } + + private static double segmentMaxRange(float lastRange, float angularResolution) { + return lastRange * Math.sin(angularResolution) / Math.sin(AUXILIARY_CONSTANT - angularResolution) + 3 * RESIDUAL_VARIANCE; + } + + public static float[] calculateSimpleVectorAngles(List Point2fs) { + if (Point2fs.size() < 5) { + return null; + } + + float[] alphas = new float[Point2fs.size()]; + for (int i = 0; i < Point2fs.size(); i++) { + Point2f before = i == 0 ? Point2fs.get(0) : Point2fs.get(i - 1); + Point2f center = Point2fs.get(i); + Point2f following = i == Point2fs.size() - 1 ? Point2fs.get(i) : Point2fs.get(i + 1); + alphas[i] = calculateVectorAngle(before, center, following); + } + return alphas; + } + + public static FeatureSet getFeatures(List sample, float angularResolution) { + return extractFeatures(sample, angularResolution); + } + + private static FeatureSet extractFeatures(List sample, float angularResolution) { + List> segments = segment(sample, angularResolution); + List corners = new ArrayList<>(); + List Line2fs = new ArrayList<>(); + for (List Point2fs : segments) { + if (Point2fs.size() < MIN_Line2D_SAMPLES) { + continue; + } + float[] deltaAngles = calculateSamplePoint2DDeltaAngles(Point2fs); + if (deltaAngles == null) { + continue; + } + Line2fs.addAll(extractLine2Ds(Point2fs, deltaAngles)); + corners.addAll(extractCorners(Point2fs, deltaAngles)); + } return new FeatureSet(Line2fs, corners); - } - - @SuppressWarnings("unused") - private static Collection extractCornersOld(List Point2fs, float[] deltaAngles) { - List corners = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length; i++) { - if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { - int maxIndex = i; - float maxPhi = deltaAngles[i]; - int j = i + 1; - float totalPhi = maxPhi; - while (j < deltaAngles.length - 1) { - if (Math.abs(deltaAngles[j]) > CURVATURE_THRESHOLD && Math.signum(deltaAngles[i]) == Math.signum(deltaAngles[j])) { - totalPhi += deltaAngles[j]; - if (deltaAngles[j] > maxPhi) { - maxPhi = deltaAngles[j]; - maxIndex = j; - } - j++; - } else { - i = j; - break; - } - } - - if (Math.abs(totalPhi) > CORNER_THRESHOLD) { - corners.add(CurvaturePoint2f.fromPoint(Point2fs.get(maxIndex), totalPhi)); - } - } - } - return corners; - } - - private static Collection extractCorners(List Point2fs, float[] deltaAngles) { - List corners = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length; i++) { - if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { - int maxIndex = i; - float maxPhi = deltaAngles[i]; - float totalPhi = maxPhi; - int last = Math.min(i + 4, deltaAngles.length); - for (int k = i + 1; k < last; k++) { - totalPhi += deltaAngles[k]; - if (deltaAngles[k] > maxPhi) { - maxPhi = deltaAngles[k]; - maxIndex = k; - } - i = k; - } - - if (Math.abs(totalPhi) > CORNER_THRESHOLD && Math.signum(totalPhi) == Math.signum(maxPhi) && maxIndex - 3 >= 0 - && maxIndex + 4 < deltaAngles.length) { - Point2f p = Point2fs.get(maxIndex); - Point2f b = Point2fs.get(maxIndex - 3); - Point2f f = Point2fs.get(maxIndex + 3); - float cornerAlpha = calculateVectorAngle(b, p, f); - if (cornerAlpha > CORNER_THRESHOLD) { - corners.add(CurvaturePoint2f.fromPoint(p, cornerAlpha)); - } - } - } - } - return corners; - } - - public static float[] calculateSamplePoint2DDeltaAngles(List Point2fs) { - if (Point2fs.size() < 5) { - return null; - } - - float[] alphas = new float[Point2fs.size()]; - for (int i = 0; i < Point2fs.size(); i++) { - if (i == 0 || i == Point2fs.size() - 1) { - alphas[i] = 0; - continue; - } - int kb = calculateKB(Point2fs, i); - int kf = calculateKF(Point2fs, i); - Point2f before = Point2fs.get(i - kb); - Point2f center = Point2fs.get(i); - Point2f following = Point2fs.get(i + kf); - alphas[i] = calculateVectorAngle(before, center, following); - } - return alphas; - } - - public static int calculateKF(List Point2fs, int Point2DIndex) { - if (Point2DIndex >= Point2fs.size() - 1) { - return 0; - } - double length = 0; - double distance = 0; - Point2f startPoint2D = Point2fs.get(Point2DIndex); - int i = Point2DIndex; - while (i < Point2fs.size() - 1) { - length += Point2fs.get(i + 1).distance(Point2fs.get(i)); - distance = Point2fs.get(i + 1).distance(startPoint2D); - if ((length - Uk) >= distance) { - break; - } - i++; - } - return i - Point2DIndex; - } - - public static int calculateKB(List Point2fs, int Point2DIndex) { - if (Point2DIndex < 1) { - return 0; - } - float length = 0; - float distance = 0; - Point2f startPoint2D = Point2fs.get(Point2DIndex); - int i = Point2DIndex; - while (i > 0) { - length += Point2fs.get(i - 1).distance(Point2fs.get(i)); - distance = Point2fs.get(i - 1).distance(startPoint2D); - if ((length - Uk) >= distance) { - break; - } - i--; - } - return Point2DIndex - i; - } - - public static void main(String[] args) { - Point2f b = Point2f.fromPolar(18, 18); - Point2f center = Point2f.fromPolar(19, 19); - Point2f f = Point2f.fromPolar(20, 20); - float radians = calculateVectorAngle(b, center, f); - - System.out.println("Vec angle: " + Math.toDegrees(radians) + " radians: " + radians); - } - - private static List extractLine2Ds(List Point2fs, float[] deltaAngles) { - List Line2fs = new ArrayList<>(); - for (int i = 0; i < deltaAngles.length - MIN_Line2D_SAMPLES;) { - while (i < deltaAngles.length - 1 && Math.abs(deltaAngles[i]) > Line2D_ANGLE_THRESHOLD) { - i++; - } - int j = i; - while (j < deltaAngles.length - 2 && (Math.abs(deltaAngles[j]) <= Line2D_ANGLE_THRESHOLD)) { - j++; - } - if (j - i - 1 >= MIN_Line2D_SAMPLES) { - Line2fs.add(new Line2f(Point2fs.get(i), Point2fs.get(j))); - } - i = j; - } - return Line2fs; - } - - public static float getAngularResolution(List Point2fs) { - return Point2fs.get(1).getAngle() - Point2fs.get(0).getAngle(); - } + } + + @SuppressWarnings("unused") + private static Collection extractCornersOld(List Point2fs, float[] deltaAngles) { + List corners = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length; i++) { + if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { + int maxIndex = i; + float maxPhi = deltaAngles[i]; + int j = i + 1; + float totalPhi = maxPhi; + while (j < deltaAngles.length - 1) { + if (Math.abs(deltaAngles[j]) > CURVATURE_THRESHOLD && Math.signum(deltaAngles[i]) == Math.signum(deltaAngles[j])) { + totalPhi += deltaAngles[j]; + if (deltaAngles[j] > maxPhi) { + maxPhi = deltaAngles[j]; + maxIndex = j; + } + j++; + } else { + i = j; + break; + } + } + + if (Math.abs(totalPhi) > CORNER_THRESHOLD) { + corners.add(CurvaturePoint2f.fromPoint(Point2fs.get(maxIndex), totalPhi)); + } + } + } + return corners; + } + + private static Collection extractCorners(List Point2fs, float[] deltaAngles) { + List corners = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length; i++) { + if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) { + int maxIndex = i; + float maxPhi = deltaAngles[i]; + float totalPhi = maxPhi; + int last = Math.min(i + 4, deltaAngles.length); + for (int k = i + 1; k < last; k++) { + totalPhi += deltaAngles[k]; + if (deltaAngles[k] > maxPhi) { + maxPhi = deltaAngles[k]; + maxIndex = k; + } + i = k; + } + + if (Math.abs(totalPhi) > CORNER_THRESHOLD && Math.signum(totalPhi) == Math.signum(maxPhi) && maxIndex - 3 >= 0 + && maxIndex + 4 < deltaAngles.length) { + Point2f p = Point2fs.get(maxIndex); + Point2f b = Point2fs.get(maxIndex - 3); + Point2f f = Point2fs.get(maxIndex + 3); + float cornerAlpha = calculateVectorAngle(b, p, f); + if (cornerAlpha > CORNER_THRESHOLD) { + corners.add(CurvaturePoint2f.fromPoint(p, cornerAlpha)); + } + } + } + } + return corners; + } + + public static float[] calculateSamplePoint2DDeltaAngles(List Point2fs) { + if (Point2fs.size() < 5) { + return null; + } + + float[] alphas = new float[Point2fs.size()]; + for (int i = 0; i < Point2fs.size(); i++) { + if (i == 0 || i == Point2fs.size() - 1) { + alphas[i] = 0; + continue; + } + int kb = calculateKB(Point2fs, i); + int kf = calculateKF(Point2fs, i); + Point2f before = Point2fs.get(i - kb); + Point2f center = Point2fs.get(i); + Point2f following = Point2fs.get(i + kf); + alphas[i] = calculateVectorAngle(before, center, following); + } + return alphas; + } + + public static int calculateKF(List Point2fs, int Point2DIndex) { + if (Point2DIndex >= Point2fs.size() - 1) { + return 0; + } + double length = 0; + double distance = 0; + Point2f startPoint2D = Point2fs.get(Point2DIndex); + int i = Point2DIndex; + while (i < Point2fs.size() - 1) { + length += Point2fs.get(i + 1).distance(Point2fs.get(i)); + distance = Point2fs.get(i + 1).distance(startPoint2D); + if ((length - Uk) >= distance) { + break; + } + i++; + } + return i - Point2DIndex; + } + + public static int calculateKB(List Point2fs, int Point2DIndex) { + if (Point2DIndex < 1) { + return 0; + } + float length = 0; + float distance = 0; + Point2f startPoint2D = Point2fs.get(Point2DIndex); + int i = Point2DIndex; + while (i > 0) { + length += Point2fs.get(i - 1).distance(Point2fs.get(i)); + distance = Point2fs.get(i - 1).distance(startPoint2D); + if ((length - Uk) >= distance) { + break; + } + i--; + } + return Point2DIndex - i; + } + + // TODO: consider as an example to remove main + public static void main(String[] args) { + Point2f b = Point2f.fromPolar(18, 18); + Point2f center = Point2f.fromPolar(19, 19); + Point2f f = Point2f.fromPolar(20, 20); + float radians = calculateVectorAngle(b, center, f); + + LOGGER.debug("Vec angle: {} radians: {}", Math.toDegrees(radians), radians); + } + + private static List extractLine2Ds(List Point2fs, float[] deltaAngles) { + List Line2fs = new ArrayList<>(); + for (int i = 0; i < deltaAngles.length - MIN_Line2D_SAMPLES; ) { + while (i < deltaAngles.length - 1 && Math.abs(deltaAngles[i]) > Line2D_ANGLE_THRESHOLD) { + i++; + } + int j = i; + while (j < deltaAngles.length - 2 && (Math.abs(deltaAngles[j]) <= Line2D_ANGLE_THRESHOLD)) { + j++; + } + if (j - i - 1 >= MIN_Line2D_SAMPLES) { + Line2fs.add(new Line2f(Point2fs.get(i), Point2fs.get(j))); + } + i = j; + } + return Line2fs; + } + + public static float getAngularResolution(List Point2fs) { + return Point2fs.get(1).getAngle() - Point2fs.get(0).getAngle(); + } } diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java index fcfc6477..fbdd5e5d 100644 --- a/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java +++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/MatrixEmpty.java @@ -17,13 +17,12 @@ package com.robo4j.math.geometry; -import java.lang.System.Logger; - /** - * Empty Matrix dimensions equal to zero + * Empty Matrix dimensions equal to zero */ public class MatrixEmpty implements Matrix { public static final int DIMENSION_ZERO = 0; + @Override public int getRows() { return DIMENSION_ZERO; diff --git a/robo4j-math/src/main/java/module-info.java b/robo4j-math/src/main/java/module-info.java index 861f3d97..cef19ea4 100644 --- a/robo4j-math/src/main/java/module-info.java +++ b/robo4j-math/src/main/java/module-info.java @@ -20,6 +20,7 @@ */ module robo4j.math { requires transitive jdk.jfr; + requires org.slf4j; exports com.robo4j.math.features; exports com.robo4j.math.geometry; diff --git a/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java b/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java index ffc3a9ca..03be3ac1 100644 --- a/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java +++ b/robo4j-math/src/test/java/com/robo4j/math/geometry/MatrixTest.java @@ -18,165 +18,172 @@ package com.robo4j.math.geometry; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static org.junit.jupiter.api.Assertions.assertEquals; /** * Matrix tests. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class MatrixTest { - static Integer [] TESTVALUES_3D_INT = new Integer[] {2, 3, 5, 7, 11, 13, 17, 19, 23}; - static Float [] TESTVALUES_3D_FLOAT = new Float[] {2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f}; - static Double [] TESTVALUES_3D_DOUBLE = new Double[] {2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0}; - static Integer [] TESTVALUES_4D_INT = new Integer[] {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53}; - static Float [] TESTVALUES_4D_FLOAT = new Float[] {2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f, 29.0f, 31.0f, 37.0f, 41.0f, 43.0f, 47.0f, 53.0f}; - static Double [] TESTVALUES_4D_DOUBLE = new Double[] {2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0, 29.0, 31.0, 37.0, 41.0, 43.0, 47.0, 53.0}; - - @Test - public void testCreation() { - Matrix matrix = createMatrix(TESTVALUES_3D_INT); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_INT); - - matrix = createMatrix(TESTVALUES_3D_FLOAT); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_FLOAT); - - matrix = createMatrix(TESTVALUES_3D_DOUBLE); - assertEquals(3, matrix.getRows()); - assertEquals(3, matrix.getColumns()); - validate(matrix, TESTVALUES_3D_DOUBLE); - - // 4D - matrix = createMatrix(TESTVALUES_4D_INT); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_INT); - - matrix = createMatrix(TESTVALUES_4D_FLOAT); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_FLOAT); - - matrix = createMatrix(TESTVALUES_4D_DOUBLE); - assertEquals(4, matrix.getRows()); - assertEquals(4, matrix.getColumns()); - validate(matrix, TESTVALUES_4D_DOUBLE); - } - - @Test - void testTranspose() { - Matrix matrix = createMatrix(TESTVALUES_3D_INT); - Matrix matrixTransposed = createMatrix(TESTVALUES_3D_INT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_3D_FLOAT); - matrixTransposed = createMatrix(TESTVALUES_3D_FLOAT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_3D_DOUBLE); - matrixTransposed = createMatrix(TESTVALUES_3D_DOUBLE); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - // 4D - matrix = createMatrix(TESTVALUES_4D_INT); - matrixTransposed = createMatrix(TESTVALUES_4D_INT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_4D_FLOAT); - matrixTransposed = createMatrix(TESTVALUES_4D_FLOAT); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - - matrix = createMatrix(TESTVALUES_4D_DOUBLE); - matrixTransposed = createMatrix(TESTVALUES_4D_DOUBLE); - matrixTransposed.transpose(); - validateTranspose(matrix, matrixTransposed); - } - - @Test - void testSysOut() { - System.out.println(createMatrix(TESTVALUES_3D_INT)); - System.out.println(createMatrix(TESTVALUES_3D_FLOAT)); - System.out.println(createMatrix(TESTVALUES_3D_DOUBLE)); - System.out.println(createMatrix(TESTVALUES_4D_INT)); - System.out.println(createMatrix(TESTVALUES_4D_FLOAT)); - System.out.println(createMatrix(TESTVALUES_4D_DOUBLE)); - } - - private void validate(Matrix matrix, Number[] numbers) { - for (int i = 0; i < matrix.getRows(); i++) { - for (int j = 0; j < matrix.getColumns(); j++) { - assertEquals(numbers[i * matrix.getColumns() + j], matrix.getNumber(i, j)); - } - } - } - - private void validateTranspose(Matrix matrix, Matrix transpose) { - for (int i = 0; i < matrix.getRows(); i++) { - for (int j = 0; j < matrix.getColumns(); j++) { - assertEquals(matrix.getNumber(i, j), transpose.getNumber(j, i)); - } - } - } - - - private Matrix createMatrix(Number[] testValues) { - if (testValues.length == 9) { - if (testValues.getClass() == Integer[].class) { - return new Matrix3i(toIntArray(testValues)); - } - if (testValues.getClass() == Float[].class) { - return new Matrix3f(toFloatArray(testValues)); - } - if (testValues.getClass() == Double[].class) { - return new Matrix3d(toDoubleArray(testValues)); - } - } - if (testValues.length == 16) { - if (testValues.getClass() == Integer[].class) { - return new Matrix4i(toIntArray(testValues)); - } - if (testValues.getClass() == Float[].class) { - return new Matrix4f(toFloatArray(testValues)); - } - if (testValues.getClass() == Double[].class) { - return new Matrix4d(toDoubleArray(testValues)); - } - } - - return null; - } - - private static int[] toIntArray(Number[] testValues) { - int [] vals = new int[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - private static float[] toFloatArray(Number[] testValues) { - float [] vals = new float[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - private static double[] toDoubleArray(Number[] testValues) { - double [] vals = new double[testValues.length]; - for (int i = 0; i < vals.length; i++) { - vals[i] = testValues[i].intValue(); - } - return vals; - } - + private static final Logger LOGGER = LoggerFactory.getLogger(MatrixTest.class); + + static Integer[] TESTVALUES_3D_INT = new Integer[]{2, 3, 5, 7, 11, 13, 17, 19, 23}; + static Float[] TESTVALUES_3D_FLOAT = new Float[]{2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f}; + static Double[] TESTVALUES_3D_DOUBLE = new Double[]{2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0}; + static Integer[] TESTVALUES_4D_INT = new Integer[]{2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53}; + static Float[] TESTVALUES_4D_FLOAT = new Float[]{2.0f, 3.0f, 5.0f, 7.0f, 11.0f, 13.0f, 17.0f, 19.0f, 23.0f, 29.0f, 31.0f, 37.0f, 41.0f, 43.0f, 47.0f, 53.0f}; + static Double[] TESTVALUES_4D_DOUBLE = new Double[]{2.0, 3.0, 5.0, 7.0, 11.0, 13.0, 17.0, 19.0, 23.0, 29.0, 31.0, 37.0, 41.0, 43.0, 47.0, 53.0}; + + // TODO : improve test structure + @Test + public void testCreation() { + Matrix matrix = createMatrix(TESTVALUES_3D_INT); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_INT); + + matrix = createMatrix(TESTVALUES_3D_FLOAT); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_FLOAT); + + matrix = createMatrix(TESTVALUES_3D_DOUBLE); + assertEquals(3, matrix.getRows()); + assertEquals(3, matrix.getColumns()); + validate(matrix, TESTVALUES_3D_DOUBLE); + + // 4D + matrix = createMatrix(TESTVALUES_4D_INT); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_INT); + + matrix = createMatrix(TESTVALUES_4D_FLOAT); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_FLOAT); + + matrix = createMatrix(TESTVALUES_4D_DOUBLE); + assertEquals(4, matrix.getRows()); + assertEquals(4, matrix.getColumns()); + validate(matrix, TESTVALUES_4D_DOUBLE); + } + + @Test + void testTranspose() { + Matrix matrix = createMatrix(TESTVALUES_3D_INT); + Matrix matrixTransposed = createMatrix(TESTVALUES_3D_INT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_3D_FLOAT); + matrixTransposed = createMatrix(TESTVALUES_3D_FLOAT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_3D_DOUBLE); + matrixTransposed = createMatrix(TESTVALUES_3D_DOUBLE); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + // 4D + matrix = createMatrix(TESTVALUES_4D_INT); + matrixTransposed = createMatrix(TESTVALUES_4D_INT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_4D_FLOAT); + matrixTransposed = createMatrix(TESTVALUES_4D_FLOAT); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + + matrix = createMatrix(TESTVALUES_4D_DOUBLE); + matrixTransposed = createMatrix(TESTVALUES_4D_DOUBLE); + matrixTransposed.transpose(); + validateTranspose(matrix, matrixTransposed); + } + + @Test + void testSysOut() { + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_INT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_FLOAT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_3D_DOUBLE)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_INT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_FLOAT)); + LOGGER.debug("{}", createMatrix(TESTVALUES_4D_DOUBLE)); + } + + private void validate(Matrix matrix, Number[] numbers) { + for (int i = 0; i < matrix.getRows(); i++) { + for (int j = 0; j < matrix.getColumns(); j++) { + assertEquals(numbers[i * matrix.getColumns() + j], matrix.getNumber(i, j)); + } + } + } + + private void validateTranspose(Matrix matrix, Matrix transpose) { + for (int i = 0; i < matrix.getRows(); i++) { + for (int j = 0; j < matrix.getColumns(); j++) { + assertEquals(matrix.getNumber(i, j), transpose.getNumber(j, i)); + } + } + } + + + private Matrix createMatrix(Number[] testValues) { + if (testValues.length == 9) { + if (testValues.getClass() == Integer[].class) { + return new Matrix3i(toIntArray(testValues)); + } + if (testValues.getClass() == Float[].class) { + return new Matrix3f(toFloatArray(testValues)); + } + if (testValues.getClass() == Double[].class) { + return new Matrix3d(toDoubleArray(testValues)); + } + } + if (testValues.length == 16) { + if (testValues.getClass() == Integer[].class) { + return new Matrix4i(toIntArray(testValues)); + } + if (testValues.getClass() == Float[].class) { + return new Matrix4f(toFloatArray(testValues)); + } + if (testValues.getClass() == Double[].class) { + return new Matrix4d(toDoubleArray(testValues)); + } + } + + return null; + } + + private static int[] toIntArray(Number[] testValues) { + int[] vals = new int[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + + private static float[] toFloatArray(Number[] testValues) { + float[] vals = new float[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + + private static double[] toDoubleArray(Number[] testValues) { + double[] vals = new double[testValues.length]; + for (int i = 0; i < vals.length; i++) { + vals[i] = testValues[i].intValue(); + } + return vals; + } + } From 6f7d40748ebdb34977dc30d6c64aaf943a54386d Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Mon, 7 Oct 2024 22:58:02 +0200 Subject: [PATCH 05/13] [69] robo4j-socket-http partial --- .../WriteDatagramSelectionKeyHandler.java | 37 +- .../src/main/java/module-info.java | 1 + .../socket/http/test/HttpHeaderTests.java | 76 +-- .../http/test/json/JsonCodecsTests.java | 424 ++++++------ .../http/test/json/JsonReaderArrayTests.java | 396 +++++------ .../http/test/json/JsonReaderTests.java | 615 +++++++++--------- .../socket/http/test/json/JsonTest.java | 261 ++++---- .../socket/http/test/json/TimeUtils.java | 9 +- .../message/DatagramDecoratedRequestTest.java | 30 +- .../http/test/request/ByteBufferTests.java | 199 +++--- .../http/test/units/HttpContextTests.java | 126 ++-- .../src/test/java/module-info.java | 1 + 12 files changed, 1101 insertions(+), 1074 deletions(-) diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/WriteDatagramSelectionKeyHandler.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/WriteDatagramSelectionKeyHandler.java index 66796bff..e3038f70 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/WriteDatagramSelectionKeyHandler.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/WriteDatagramSelectionKeyHandler.java @@ -20,6 +20,8 @@ import com.robo4j.RoboReference; import com.robo4j.socket.http.request.DatagramResponseProcess; import com.robo4j.socket.http.units.ServerContext; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.nio.channels.SelectionKey; import java.util.Map; @@ -29,20 +31,21 @@ * @author Miroslav Wengner (@miragemiko) */ public class WriteDatagramSelectionKeyHandler implements SelectionKeyHandler { - private final Map outBuffers; - private final SelectionKey key; + private static final Logger LOGGER = LoggerFactory.getLogger(WriteDatagramSelectionKeyHandler.class); + private final Map outBuffers; + private final SelectionKey key; - public WriteDatagramSelectionKeyHandler(RoboContext context, ServerContext serverContext, - Map outBuffers, SelectionKey key) { - this.outBuffers = outBuffers; - this.key = key; - } + public WriteDatagramSelectionKeyHandler(RoboContext context, ServerContext serverContext, + Map outBuffers, SelectionKey key) { + this.outBuffers = outBuffers; + this.key = key; + } - @Override - public SelectionKey handle() { + @Override + public SelectionKey handle() { // final DatagramChannel channel = (DatagramChannel) key.channel(); // ByteBuffer buffer = ByteBuffer.allocate(serverContext.getPropertySafe(Integer.class, PROPERTY_BUFFER_CAPACITY)); - final DatagramResponseProcess responseProcess = outBuffers.get(key); + final DatagramResponseProcess responseProcess = outBuffers.get(key); // buffer.clear(); // buffer.put("ACCEPTED".getBytes()); @@ -53,12 +56,12 @@ public SelectionKey handle() { // throw new SocketException("handle", e); // } - RoboReference reference = responseProcess.getTarget(); - Object responseMessage = responseProcess.getResult(); - reference.sendMessage(responseMessage); - System.out.println("Wrote: " + responseMessage); + RoboReference reference = responseProcess.getTarget(); + Object responseMessage = responseProcess.getResult(); + reference.sendMessage(responseMessage); + LOGGER.debug("responseMessage:{}", responseMessage); - key.cancel(); - return key; - } + key.cancel(); + return key; + } } diff --git a/robo4j-socket-http/src/main/java/module-info.java b/robo4j-socket-http/src/main/java/module-info.java index fc3dc94e..0da234bc 100644 --- a/robo4j-socket-http/src/main/java/module-info.java +++ b/robo4j-socket-http/src/main/java/module-info.java @@ -1,5 +1,6 @@ module robo4j.http { requires transitive robo4j.core; + requires org.slf4j; exports com.robo4j.socket.http; exports com.robo4j.socket.http.codec; diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/HttpHeaderTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/HttpHeaderTests.java index 0b132627..1998dc87 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/HttpHeaderTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/HttpHeaderTests.java @@ -21,7 +21,6 @@ import com.robo4j.socket.http.HttpVersion; import com.robo4j.socket.http.ProtocolType; import com.robo4j.socket.http.enums.StatusCode; -import com.robo4j.socket.http.message.HttpDenominator; import com.robo4j.socket.http.message.HttpRequestDenominator; import com.robo4j.socket.http.provider.DefaultValuesProvider; import com.robo4j.socket.http.util.HttpHeaderBuilder; @@ -29,6 +28,8 @@ import com.robo4j.socket.http.util.HttpMessageUtils; import com.robo4j.socket.http.util.RoboHttpUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static com.robo4j.socket.http.HttpHeaderFieldValues.CONNECTION_KEEP_ALIVE; import static com.robo4j.socket.http.provider.DefaultValuesProvider.ROBO4J_CLIENT; @@ -43,31 +44,32 @@ * @author Miro Wengner (@miragemiko) */ class HttpHeaderTests { + private static final Logger LOGGER = LoggerFactory.getLogger(HttpHeaderTests.class); - @Test - void createHeaderTest() { - String header = HttpHeaderBuilder.Build().addFirstLine("units/controller") - .addAll(DefaultValuesProvider.BASIC_HEADER_MAP).build(HttpMethod.GET, HttpVersion.HTTP_1_1); + @Test + void createHeaderTest() { + String header = HttpHeaderBuilder.Build().addFirstLine("units/controller") + .addAll(DefaultValuesProvider.BASIC_HEADER_MAP).build(HttpMethod.GET, HttpVersion.HTTP_1_1); - assertNotNull(header); - assertEquals(8, header.split(HTTP_NEW_LINE).length); - assertEquals(header.split(HTTP_NEW_LINE)[2], - createHeaderField(HttpHeaderFieldNames.USER_AGENT, ROBO4J_CLIENT)); - assertEquals(header.split(HTTP_NEW_LINE)[3], - createHeaderField(HttpHeaderFieldNames.CONNECTION, CONNECTION_KEEP_ALIVE)); - } + assertNotNull(header); + assertEquals(8, header.split(HTTP_NEW_LINE).length); + assertEquals(header.split(HTTP_NEW_LINE)[2], + createHeaderField(HttpHeaderFieldNames.USER_AGENT, ROBO4J_CLIENT)); + assertEquals(header.split(HTTP_NEW_LINE)[3], + createHeaderField(HttpHeaderFieldNames.CONNECTION, CONNECTION_KEEP_ALIVE)); + } - @Test - void characterParser() { - assertEquals("[", HttpMessageUtils.getHttpSeparator(13)); - assertEquals("]",HttpMessageUtils.getHttpSeparator(14)); - } + @Test + void characterParser() { + assertEquals("[", HttpMessageUtils.getHttpSeparator(13)); + assertEquals("]", HttpMessageUtils.getHttpSeparator(14)); + } - @Test - void test() { - String uid = "1234"; - String expectedResult = "HTTP/1.1 200 OK" + HTTP_NEW_LINE + "uid: " + uid + HTTP_NEW_LINE; - //@formatter:off + @Test + void test() { + String uid = "1234"; + String expectedResult = "HTTP/1.1 200 OK" + HTTP_NEW_LINE + "uid: " + uid + HTTP_NEW_LINE; + //@formatter:off String getHeader = HttpHeaderBuilder.Build() .addFirstLine(HttpVersion.HTTP_1_1.getValue()) .addFirstLine(StatusCode.OK.getCode()) @@ -75,21 +77,21 @@ void test() { .add(HttpHeaderFieldNames.ROBO_UNIT_UID, uid) .build(); //@formatter:on - assertEquals(expectedResult, getHeader); - } + assertEquals(expectedResult, getHeader); + } - @Test - void extractHeaderParameter() { - String message = "message"; - HttpDenominator denominator = new HttpRequestDenominator(HttpMethod.POST, HttpVersion.HTTP_1_1); - String postRequest = HttpMessageBuilder.Build().setDenominator(denominator) - .addHeaderElement(HttpHeaderFieldNames.HOST, RoboHttpUtils.createHost("127.0.0.1", ProtocolType.HTTP.getPort())) - .addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(message.length())) - .build(message); - System.out.println("HEADER: " + postRequest); - } + @Test + void extractHeaderParameter() { + var message = "message"; + var denominator = new HttpRequestDenominator(HttpMethod.POST, HttpVersion.HTTP_1_1); + var postRequest = HttpMessageBuilder.Build().setDenominator(denominator) + .addHeaderElement(HttpHeaderFieldNames.HOST, RoboHttpUtils.createHost("127.0.0.1", ProtocolType.HTTP.getPort())) + .addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(message.length())) + .build(message); + LOGGER.debug("postRequest:{}", postRequest); + } - private String createHeaderField(String key, String value) { - return key + ": " + value; - } + private String createHeaderField(String key, String value) { + return key + ": " + value; + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java index a97c7927..15d791c1 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java @@ -17,13 +17,8 @@ package com.robo4j.socket.http.test.json; import com.robo4j.socket.http.HttpMethod; -import com.robo4j.socket.http.test.codec.CameraMessage; -import com.robo4j.socket.http.test.codec.CameraMessageCodec; -import com.robo4j.socket.http.test.codec.HttpPathDTOCodec; -import com.robo4j.socket.http.test.codec.NSBETypesAndCollectionTestMessageCodec; -import com.robo4j.socket.http.test.codec.NSBETypesTestMessageCodec; -import com.robo4j.socket.http.test.codec.NSBWithSimpleCollectionsTypesMessageCodec; import com.robo4j.socket.http.dto.HttpPathMethodDTO; +import com.robo4j.socket.http.test.codec.*; import com.robo4j.socket.http.test.units.config.codec.NSBETypesAndCollectionTestMessage; import com.robo4j.socket.http.test.units.config.codec.NSBETypesTestMessage; import com.robo4j.socket.http.test.units.config.codec.NSBWithSimpleCollectionsTypesMessage; @@ -32,219 +27,224 @@ import com.robo4j.util.StreamUtils; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.Arrays; -import java.util.Base64; -import java.util.Collections; -import java.util.LinkedHashMap; -import java.util.Map; +import java.util.*; -import static org.junit.jupiter.api.Assertions.assertArrayEquals; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class JsonCodecsTests { - - private static String testJson = "{\"number\":42,\"message\":\"no message\",\"active\":false," - + "\"array\":[\"one\",\"two\"],\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}," - + "\"persons\":[{\"name\":\"name1\",\"value\":22,\"child\":{\"name\":\"name11\",\"value\":0," - + "\"child\":{\"name\":\"name111\",\"value\":42}}},{\"name\":\"name2\",\"value\":5}]," - + "\"personMap\":{\"person1\":{\"name\":\"name1\",\"value\":22,\"child\":{\"name\":\"name11\",\"value\":0," - + "\"child\":{\"name\":\"name111\",\"value\":42}}},\"person2\":{\"name\":\"name2\",\"value\":5}}}"; - - private NSBWithSimpleCollectionsTypesMessageCodec collectionsTypesMessageCodec; - private NSBETypesTestMessageCodec enumTypesMessageCodec; - private NSBETypesAndCollectionTestMessageCodec collectionEnumTypesMessageCodec; - private CameraMessageCodec cameraMessageCodec; - private HttpPathDTOCodec httpPathDTOCodec; - - @BeforeEach - void setUp() { - collectionsTypesMessageCodec = new NSBWithSimpleCollectionsTypesMessageCodec(); - enumTypesMessageCodec = new NSBETypesTestMessageCodec(); - collectionEnumTypesMessageCodec = new NSBETypesAndCollectionTestMessageCodec(); - cameraMessageCodec = new CameraMessageCodec(); - httpPathDTOCodec = new HttpPathDTOCodec(); - } - - @SuppressWarnings("unchecked") - @Test - void encodeServerPathDTOMessageNoFilterTest() { - // String expectedJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}"; - HttpPathMethodDTO message = new HttpPathMethodDTO(); - message.setRoboUnit("roboUnit1"); - message.setMethod(HttpMethod.GET); - message.setCallbacks(Collections.EMPTY_LIST); - - String resultJson = httpPathDTOCodec.encode(message); - HttpPathMethodDTO decodedMessage = httpPathDTOCodec.decode(resultJson); - - System.out.println("resultJson: " + resultJson); - System.out.println("decodedMessage: " + decodedMessage); - - // Assert.assertTrue(expectedJson.equals(resultJson)); - assertEquals(message, decodedMessage); - } - - @Test - void encodeMessageWithEnumTypeTest() { - String expectedJson = "{\"number\":42,\"message\":\"enum type 1\",\"active\":true,\"command\":\"MOVE\"}"; - NSBETypesTestMessage message = new NSBETypesTestMessage(); - message.setNumber(42); - message.setMessage("enum type 1"); - message.setActive(true); - message.setCommand(TestCommandEnum.MOVE); - - String resultJson = enumTypesMessageCodec.encode(message); - NSBETypesTestMessage decodedMessage = enumTypesMessageCodec.decode(resultJson); - - System.out.println("resultJson: " + resultJson); - System.out.println("decodedMessage: " + decodedMessage); - assertNotNull(resultJson); - assertEquals(expectedJson, resultJson); - assertEquals(message, decodedMessage); - } - - @Test - void encodeMessageWithEnumCollectionTypeTest() { - - String expectedJson = "{\"number\":42,\"message\":\"enum type 1\",\"active\":true,\"command\":\"MOVE\",\"commands\":[\"MOVE\",\"STOP\",\"BACK\"]}"; - NSBETypesAndCollectionTestMessage message = new NSBETypesAndCollectionTestMessage(); - message.setNumber(42); - message.setMessage("enum type 1"); - message.setActive(true); - message.setCommand(TestCommandEnum.MOVE); - message.setCommands(Arrays.asList(TestCommandEnum.MOVE, TestCommandEnum.STOP, TestCommandEnum.BACK)); - - String resultJson = collectionEnumTypesMessageCodec.encode(message); - NSBETypesAndCollectionTestMessage decodeMessage = collectionEnumTypesMessageCodec.decode(expectedJson); - - System.out.println("resultJson: " + resultJson); - System.out.println("decodeMessage: " + decodeMessage); - assertNotNull(resultJson); - assertEquals(expectedJson, resultJson); - assertEquals(message, decodeMessage); - } - - @Test - void nestedObjectToJson() { - - TestPerson testPerson2 = new TestPerson(); - testPerson2.setName("name2"); - testPerson2.setValue(5); - - TestPerson testPerson111 = new TestPerson(); - testPerson111.setName("name111"); - testPerson111.setValue(42); - - TestPerson testPerson11 = new TestPerson(); - testPerson11.setName("name11"); - testPerson11.setValue(0); - testPerson11.setChild(testPerson111); - - TestPerson testPerson1 = new TestPerson(); - testPerson1.setName("name1"); - testPerson1.setValue(22); - testPerson1.setChild(testPerson11); - - Map personMap = new LinkedHashMap<>(); - personMap.put("person1", testPerson1); - personMap.put("person2", testPerson2); - - NSBWithSimpleCollectionsTypesMessage obj1 = new NSBWithSimpleCollectionsTypesMessage(); - obj1.setNumber(42); - obj1.setMessage("no message"); - obj1.setActive(false); - obj1.setArray(new String[] { "one", "two" }); - obj1.setList(Arrays.asList("text1", "text2")); - obj1.setMap(Collections.singletonMap("key", "value")); - obj1.setPersons(Arrays.asList(testPerson1, testPerson2)); - obj1.setPersonMap(personMap); - - long start = System.nanoTime(); - String json = collectionsTypesMessageCodec.encode(obj1); - TimeUtils.printTimeDiffNano("decodeFromJson", start); - System.out.println("JSON1: " + json); - - assertTrue(testJson.equals(json)); - - } - - @Test - void nestedJsonToObject() { - - TestPerson testPerson2 = new TestPerson(); - testPerson2.setName("name2"); - testPerson2.setValue(5); - - TestPerson testPerson111 = new TestPerson(); - testPerson111.setName("name111"); - testPerson111.setValue(42); - - TestPerson testPerson11 = new TestPerson(); - testPerson11.setName("name11"); - testPerson11.setValue(0); - testPerson11.setChild(testPerson111); - - TestPerson testPerson1 = new TestPerson(); - testPerson1.setName("name1"); - testPerson1.setValue(22); - testPerson1.setChild(testPerson11); - - Map personMap = new LinkedHashMap<>(); - personMap.put("person1", testPerson1); - personMap.put("person2", testPerson2); - - long start = System.nanoTime(); - NSBWithSimpleCollectionsTypesMessage obj1 = collectionsTypesMessageCodec.decode(testJson); - TimeUtils.printTimeDiffNano("decodeFromJson", start); - - assertEquals(Integer.valueOf(42), obj1.getNumber()); - assertEquals("no message", obj1.getMessage()); - assertTrue(!obj1.getActive()); - assertArrayEquals(new String[] { "one", "two" }, obj1.getArray()); - assertTrue(obj1.getList().containsAll(Arrays.asList("text1", "text2"))); - assertEquals(personMap, obj1.getPersonMap()); - - System.out.println("Obj: " + obj1); - } - - @Test - void cameraCodecJsonCycleTest() { - - final byte[] imageBytes = StreamUtils.inputStreamToByteArray( - Thread.currentThread().getContextClassLoader().getResourceAsStream("snapshot.png")); - String encodeString = new String(Base64.getEncoder().encode(imageBytes)); - - CameraMessage cameraMessage = new CameraMessage(); - cameraMessage.setImage(encodeString); - cameraMessage.setType("jpg"); - cameraMessage.setValue("22"); - - long start = System.nanoTime(); - String cameraJson0 = cameraMessageCodec.encode(cameraMessage); - TimeUtils.printTimeDiffNano("cameraJson0", start); - - start = System.nanoTime(); - cameraMessageCodec.decode(cameraJson0); - TimeUtils.printTimeDiffNano("decodeCameraMessage0", start); - - start = System.nanoTime(); - String cameraJson = cameraMessageCodec.encode(cameraMessage); - TimeUtils.printTimeDiffNano("decodeFromJson", start); - System.out.println("cameraJson: " + cameraJson); - - start = System.nanoTime(); - CameraMessage codecCameraMessage = cameraMessageCodec.decode(cameraJson); - TimeUtils.printTimeDiffNano("decodeFromJson", start); - - assertEquals(cameraMessage, codecCameraMessage); - - } + private static final Logger LOGGER = LoggerFactory.getLogger(JsonCodecsTests.class); + + private static String testJson = "{\"number\":42,\"message\":\"no message\",\"active\":false," + + "\"array\":[\"one\",\"two\"],\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}," + + "\"persons\":[{\"name\":\"name1\",\"value\":22,\"child\":{\"name\":\"name11\",\"value\":0," + + "\"child\":{\"name\":\"name111\",\"value\":42}}},{\"name\":\"name2\",\"value\":5}]," + + "\"personMap\":{\"person1\":{\"name\":\"name1\",\"value\":22,\"child\":{\"name\":\"name11\",\"value\":0," + + "\"child\":{\"name\":\"name111\",\"value\":42}}},\"person2\":{\"name\":\"name2\",\"value\":5}}}"; + + private NSBWithSimpleCollectionsTypesMessageCodec collectionsTypesMessageCodec; + private NSBETypesTestMessageCodec enumTypesMessageCodec; + private NSBETypesAndCollectionTestMessageCodec collectionEnumTypesMessageCodec; + private CameraMessageCodec cameraMessageCodec; + private HttpPathDTOCodec httpPathDTOCodec; + + @BeforeEach + void setUp() { + collectionsTypesMessageCodec = new NSBWithSimpleCollectionsTypesMessageCodec(); + enumTypesMessageCodec = new NSBETypesTestMessageCodec(); + collectionEnumTypesMessageCodec = new NSBETypesAndCollectionTestMessageCodec(); + cameraMessageCodec = new CameraMessageCodec(); + httpPathDTOCodec = new HttpPathDTOCodec(); + } + + @SuppressWarnings("unchecked") + @Test + void encodeServerPathDTOMessageNoFilterTest() { + // String expectedJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}"; + HttpPathMethodDTO message = new HttpPathMethodDTO(); + message.setRoboUnit("roboUnit1"); + message.setMethod(HttpMethod.GET); + message.setCallbacks(Collections.EMPTY_LIST); + + String resultJson = httpPathDTOCodec.encode(message); + HttpPathMethodDTO decodedMessage = httpPathDTOCodec.decode(resultJson); + + printJson(resultJson); + printDecodedMessage(decodedMessage); + + // Assert.assertTrue(expectedJson.equals(resultJson)); + assertEquals(message, decodedMessage); + } + + @Test + void encodeMessageWithEnumTypeTest() { + String expectedJson = "{\"number\":42,\"message\":\"enum type 1\",\"active\":true,\"command\":\"MOVE\"}"; + NSBETypesTestMessage message = new NSBETypesTestMessage(); + message.setNumber(42); + message.setMessage("enum type 1"); + message.setActive(true); + message.setCommand(TestCommandEnum.MOVE); + + String resultJson = enumTypesMessageCodec.encode(message); + NSBETypesTestMessage decodedMessage = enumTypesMessageCodec.decode(resultJson); + + printJson(resultJson); + printDecodedMessage(decodedMessage); + assertNotNull(resultJson); + assertEquals(expectedJson, resultJson); + assertEquals(message, decodedMessage); + } + + @Test + void encodeMessageWithEnumCollectionTypeTest() { + + String expectedJson = "{\"number\":42,\"message\":\"enum type 1\",\"active\":true,\"command\":\"MOVE\",\"commands\":[\"MOVE\",\"STOP\",\"BACK\"]}"; + NSBETypesAndCollectionTestMessage message = new NSBETypesAndCollectionTestMessage(); + message.setNumber(42); + message.setMessage("enum type 1"); + message.setActive(true); + message.setCommand(TestCommandEnum.MOVE); + message.setCommands(Arrays.asList(TestCommandEnum.MOVE, TestCommandEnum.STOP, TestCommandEnum.BACK)); + + String resultJson = collectionEnumTypesMessageCodec.encode(message); + NSBETypesAndCollectionTestMessage decodedMessage = collectionEnumTypesMessageCodec.decode(expectedJson); + + + printJson(resultJson); + printDecodedMessage(decodedMessage); + assertNotNull(resultJson); + assertEquals(expectedJson, resultJson); + assertEquals(message, decodedMessage); + } + + @Test + void nestedObjectToJson() { + + TestPerson testPerson2 = new TestPerson(); + testPerson2.setName("name2"); + testPerson2.setValue(5); + + TestPerson testPerson111 = new TestPerson(); + testPerson111.setName("name111"); + testPerson111.setValue(42); + + TestPerson testPerson11 = new TestPerson(); + testPerson11.setName("name11"); + testPerson11.setValue(0); + testPerson11.setChild(testPerson111); + + TestPerson testPerson1 = new TestPerson(); + testPerson1.setName("name1"); + testPerson1.setValue(22); + testPerson1.setChild(testPerson11); + + Map personMap = new LinkedHashMap<>(); + personMap.put("person1", testPerson1); + personMap.put("person2", testPerson2); + + NSBWithSimpleCollectionsTypesMessage obj1 = new NSBWithSimpleCollectionsTypesMessage(); + obj1.setNumber(42); + obj1.setMessage("no message"); + obj1.setActive(false); + obj1.setArray(new String[]{"one", "two"}); + obj1.setList(Arrays.asList("text1", "text2")); + obj1.setMap(Collections.singletonMap("key", "value")); + obj1.setPersons(Arrays.asList(testPerson1, testPerson2)); + obj1.setPersonMap(personMap); + + long start = System.nanoTime(); + String json = collectionsTypesMessageCodec.encode(obj1); + TimeUtils.printTimeDiffNano("decodeFromJson", start); + printJson(json); + + assertEquals(testJson, json); + + } + + @Test + void nestedJsonToObject() { + + TestPerson testPerson2 = new TestPerson(); + testPerson2.setName("name2"); + testPerson2.setValue(5); + + TestPerson testPerson111 = new TestPerson(); + testPerson111.setName("name111"); + testPerson111.setValue(42); + + TestPerson testPerson11 = new TestPerson(); + testPerson11.setName("name11"); + testPerson11.setValue(0); + testPerson11.setChild(testPerson111); + + TestPerson testPerson1 = new TestPerson(); + testPerson1.setName("name1"); + testPerson1.setValue(22); + testPerson1.setChild(testPerson11); + + Map personMap = new LinkedHashMap<>(); + personMap.put("person1", testPerson1); + personMap.put("person2", testPerson2); + + long start = System.nanoTime(); + NSBWithSimpleCollectionsTypesMessage obj1 = collectionsTypesMessageCodec.decode(testJson); + TimeUtils.printTimeDiffNano("decodeFromJson", start); + + assertEquals(Integer.valueOf(42), obj1.getNumber()); + assertEquals("no message", obj1.getMessage()); + assertTrue(!obj1.getActive()); + assertArrayEquals(new String[]{"one", "two"}, obj1.getArray()); + assertTrue(obj1.getList().containsAll(Arrays.asList("text1", "text2"))); + assertEquals(personMap, obj1.getPersonMap()); + + System.out.println("Obj: " + obj1); + } + + @Test + void cameraCodecJsonCycleTest() { + + final byte[] imageBytes = StreamUtils.inputStreamToByteArray( + Thread.currentThread().getContextClassLoader().getResourceAsStream("snapshot.png")); + String encodeString = new String(Base64.getEncoder().encode(imageBytes)); + + CameraMessage cameraMessage = new CameraMessage(); + cameraMessage.setImage(encodeString); + cameraMessage.setType("jpg"); + cameraMessage.setValue("22"); + + long start = System.nanoTime(); + String cameraJson0 = cameraMessageCodec.encode(cameraMessage); + TimeUtils.printTimeDiffNano("cameraJson0", start); + + start = System.nanoTime(); + cameraMessageCodec.decode(cameraJson0); + TimeUtils.printTimeDiffNano("decodeCameraMessage0", start); + + start = System.nanoTime(); + String cameraJson = cameraMessageCodec.encode(cameraMessage); + TimeUtils.printTimeDiffNano("decodeFromJson", start); + printJson(cameraJson); + + start = System.nanoTime(); + CameraMessage codecCameraMessage = cameraMessageCodec.decode(cameraJson); + TimeUtils.printTimeDiffNano("decodeFromJson", start); + + assertEquals(cameraMessage, codecCameraMessage); + + } + + private static void printDecodedMessage(T decodedMessage) { + LOGGER.debug("decodedMessage:{}", decodedMessage); + } + + private static void printJson(String resultJson) { + LOGGER.debug("resultJson:{}", resultJson); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderArrayTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderArrayTests.java index 23f0e7a5..95ae3b52 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderArrayTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderArrayTests.java @@ -19,11 +19,14 @@ import com.robo4j.socket.http.json.JsonDocument; import com.robo4j.socket.http.json.JsonReader; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.Collections; import java.util.List; +import static com.robo4j.socket.http.test.json.JsonReaderTests.printDocument; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -32,200 +35,201 @@ * @author Miroslav Wengner (@miragemiko) */ class JsonReaderArrayTests { - - private static final String jsonArrayStringOnly = "[\"one\", \"two\" , \"three\"]"; - private static final String jsonArrayIntegerOnly = "[ 1,2,3 ,4, 5 , 6, 7]"; - private static final String jsonArrayBooleanOnly = "[ true, false, false, false, true]"; - private static final String jsonArrayNumberOnly = "[ 0.2, 0.1, 0.3]"; - private static final String jsonArrayObjectOnly = "[{\"name\":\"name1\",\"value\":42}, {\"name\":\"name2\",\"value\":22}, " - + "{\"name\":\"name3\",\"value\":8}]"; - private static final String jsonArrayOfArraysStringsOnly = "[[\"one\" ,\"two\", \"three\"] , [\"one\",\"two\"],[\"one\"]]"; - private static final String jsonArrayOfArraysIntegersOnly = "[[ 1 , 2, 3] , [1,2],[1]]"; - private static final String jsonArrayOfArraysBooleansOnly = "[[ true, false, true] , [true, false],[true]]"; - private static final String jsonArrayOfArraysObjectsOnly = "[[ {\"name\":\"name11\",\"value\":11}, " - + "{\"name\":\"name12\",\"value\":12}, {\"name\":\"name13\",\"value\":13}] , [{\"name\":\"name21\",\"value\":21}, " - + "{\"name\":\"name22\",\"value\":22}],[{\"name\":\"name31\",\"value\":31}]]"; - - private static final String jsonArrayOfArraysObjectsWithObjectWithArray = "[[ {\"name\":\"name11\",\"value\":11}, " - + "{\"name\":\"name12\",\"value\":12}, {\"name\":\"name13\",\"value\":13}] , [{\"name\":\"name21\",\"value\":21}, " - + "{\"name\":\"name22\",\"value\":22}],[{\"name\":\"name31\",\"value\":31, \"array\":[31,32,33], " - + "\"child\":{\"name\":\"name311\", \"array\":[31,32,33]}}]]"; - - @Test - void basicArrayStringOnlyTest() { - JsonReader parser = new JsonReader(jsonArrayStringOnly); - JsonDocument document = parser.read(); - - System.out.println("DOC: " + document); - List testArray = Arrays.asList("one", "two", "three"); - assertEquals(JsonDocument.Type.ARRAY, document.getType()); - assertEquals(testArray.size(), document.getArray().size()); - assertTrue(document.getArray().containsAll(testArray)); - } - - @Test - void basicArrayBooleanOnlyTest() { - JsonReader parser = new JsonReader(jsonArrayBooleanOnly); - JsonDocument document = parser.read(); - - List testArray = Arrays.asList(true, false, false, false, true); - assertEquals(JsonDocument.Type.ARRAY, document.getType()); - assertEquals(testArray.size(), document.getArray().size()); - assertTrue(document.getArray().containsAll(testArray)); - } - - @Test - void basicArrayIntegerOnlyTest() { - JsonReader parser = new JsonReader(jsonArrayIntegerOnly); - JsonDocument document = parser.read(); - - System.out.println("DOC: " + document); - List testArray = Arrays.asList(1, 2, 3, 4, 5, 6, 7); - assertEquals(JsonDocument.Type.ARRAY, document.getType()); - assertEquals(testArray.size(), document.getArray().size()); - assertTrue(document.getArray().containsAll(testArray)); - } - - @Test - void basicArrayNumberOnlyTest() { - JsonReader parser = new JsonReader(jsonArrayNumberOnly); - JsonDocument document = parser.read(); - - List testArray = Arrays.asList(0.2, 0.1, 0.3); - assertEquals(JsonDocument.Type.ARRAY, document.getType()); - assertEquals(testArray.size(), document.getArray().size()); - assertTrue(document.getArray().containsAll(testArray)); - } - - @Test - void basicArrayObjectOnlyTest() { - - JsonDocument jsonDocumentChild1 = new JsonDocument(JsonDocument.Type.OBJECT); - jsonDocumentChild1.put("name", "name1"); - jsonDocumentChild1.put("value", 42); - - JsonDocument jsonDocumentChild2 = new JsonDocument(JsonDocument.Type.OBJECT); - jsonDocumentChild2.put("name", "name2"); - jsonDocumentChild2.put("value", 22); - - JsonDocument jsonDocumentChild3 = new JsonDocument(JsonDocument.Type.OBJECT); - jsonDocumentChild3.put("name", "name3"); - jsonDocumentChild3.put("value", 8); - - List expectedArrayResult = Arrays.asList(jsonDocumentChild1, jsonDocumentChild2, - jsonDocumentChild3); - - JsonReader parser = new JsonReader(jsonArrayObjectOnly); - JsonDocument document = parser.read(); - - System.out.println("DOC: " + document); - assertEquals(JsonDocument.Type.ARRAY, document.getType()); - assertEquals(expectedArrayResult.size(), document.getArray().size()); - assertTrue(document.getArray().containsAll(expectedArrayResult)); - } - - @Test - void basicArrayOfArrayStringOnlyTest() { - - List stringArray1 = Arrays.asList("one", "two", "three"); - List stringArray2 = Arrays.asList("one", "two"); - List stringArray3 = Collections.singletonList("one"); - JsonReader parser = new JsonReader(jsonArrayOfArraysStringsOnly); - JsonDocument document = parser.read(); - - assertTrue(compareArrays(stringArray1, document, 0)); - assertTrue(compareArrays(stringArray2, document, 1)); - assertTrue(compareArrays(stringArray3, document, 2)); - System.out.println("DOC: " + document); - } - - @Test - void basicArrayOfArraysIntegersOnlyTest() { - List stringArray1 = Arrays.asList(1, 2, 3); - List stringArray2 = Arrays.asList(1, 2); - List stringArray3 = Collections.singletonList(1); - - JsonReader parser = new JsonReader(jsonArrayOfArraysIntegersOnly); - JsonDocument document = parser.read(); - - assertTrue(compareArrays(stringArray1, document, 0)); - assertTrue(compareArrays(stringArray2, document, 1)); - assertTrue(compareArrays(stringArray3, document, 2)); - System.out.println("DOC: " + document); - } - - @Test - void basicArrayOfArraysBooleanOnlyTest() { - List stringArray1 = Arrays.asList(true, false, true); - List stringArray2 = Arrays.asList(true, false); - List stringArray3 = Collections.singletonList(true); - - JsonReader parser = new JsonReader(jsonArrayOfArraysBooleansOnly); - JsonDocument document = parser.read(); - - assertTrue(compareArrays(stringArray1, document, 0)); - assertTrue(compareArrays(stringArray2, document, 1)); - assertTrue(compareArrays(stringArray3, document, 2)); - System.out.println("DOC: " + document); - } - - @Test - void basicArrayOfArraysObjectsOnlyTest() { - List array1 = Arrays.asList(testObjectJsonDocument(11), testObjectJsonDocument(12), - testObjectJsonDocument(13)); - List array2 = Arrays.asList(testObjectJsonDocument(21), testObjectJsonDocument(22)); - List array3 = Collections.singletonList(testObjectJsonDocument(31)); - - JsonReader parser = new JsonReader(jsonArrayOfArraysObjectsOnly); - JsonDocument document = parser.read(); - - assertTrue(compareArrays(array1, document, 0)); - assertTrue(compareArrays(array2, document, 1)); - assertTrue(compareArrays(array3, document, 2)); - - System.out.println("DOC: " + document); - } - - @Test - void basicArrayOfArraysObjectsWithObjectWithArrayTest() { - List array1 = Arrays.asList(testObjectJsonDocument(11), testObjectJsonDocument(12), - testObjectJsonDocument(13)); - List array2 = Arrays.asList(testObjectJsonDocument(21), testObjectJsonDocument(22)); - - JsonDocument lastJsonDocument = testObjectJsonDocument(31); - JsonDocument lastJsonDocumentArray = new JsonDocument(JsonDocument.Type.ARRAY); - lastJsonDocumentArray.add(31); - lastJsonDocumentArray.add(32); - lastJsonDocumentArray.add(33); - JsonDocument lastJsonDocumentObject = new JsonDocument(JsonDocument.Type.OBJECT); - lastJsonDocumentObject.put("name", "name311"); - lastJsonDocumentObject.put("array", lastJsonDocumentArray); - lastJsonDocument.put("array", lastJsonDocumentArray); - lastJsonDocument.put("child", lastJsonDocumentObject); - List array3 = Collections.singletonList(lastJsonDocument); - - JsonReader parser = new JsonReader(jsonArrayOfArraysObjectsWithObjectWithArray); - long start = System.nanoTime(); - JsonDocument document = parser.read(); - TimeUtils.printTimeDiffNano("robo4j:", start); - - assertTrue(compareArrays(array1, document, 0)); - assertTrue(compareArrays(array2, document, 1)); - assertTrue(compareArrays(array3, document, 2)); - - System.out.println("DOC: " + document); - } - - private boolean compareArrays(List sourceArray, JsonDocument arrayDocument, int index) { - JsonDocument desiredArrayDocument = (JsonDocument) arrayDocument.getArray().get(index); - assertEquals(JsonDocument.Type.ARRAY, desiredArrayDocument.getType()); - return sourceArray.containsAll(desiredArrayDocument.getArray()); - } - - private JsonDocument testObjectJsonDocument(Integer value) { - JsonDocument result = new JsonDocument(JsonDocument.Type.OBJECT); - result.put("name", "name" + value); - result.put("value", value); - return result; - } + private static final Logger LOGGER = LoggerFactory.getLogger(JsonReaderArrayTests.class); + + private static final String jsonArrayStringOnly = "[\"one\", \"two\" , \"three\"]"; + private static final String jsonArrayIntegerOnly = "[ 1,2,3 ,4, 5 , 6, 7]"; + private static final String jsonArrayBooleanOnly = "[ true, false, false, false, true]"; + private static final String jsonArrayNumberOnly = "[ 0.2, 0.1, 0.3]"; + private static final String jsonArrayObjectOnly = "[{\"name\":\"name1\",\"value\":42}, {\"name\":\"name2\",\"value\":22}, " + + "{\"name\":\"name3\",\"value\":8}]"; + private static final String jsonArrayOfArraysStringsOnly = "[[\"one\" ,\"two\", \"three\"] , [\"one\",\"two\"],[\"one\"]]"; + private static final String jsonArrayOfArraysIntegersOnly = "[[ 1 , 2, 3] , [1,2],[1]]"; + private static final String jsonArrayOfArraysBooleansOnly = "[[ true, false, true] , [true, false],[true]]"; + private static final String jsonArrayOfArraysObjectsOnly = "[[ {\"name\":\"name11\",\"value\":11}, " + + "{\"name\":\"name12\",\"value\":12}, {\"name\":\"name13\",\"value\":13}] , [{\"name\":\"name21\",\"value\":21}, " + + "{\"name\":\"name22\",\"value\":22}],[{\"name\":\"name31\",\"value\":31}]]"; + + private static final String jsonArrayOfArraysObjectsWithObjectWithArray = "[[ {\"name\":\"name11\",\"value\":11}, " + + "{\"name\":\"name12\",\"value\":12}, {\"name\":\"name13\",\"value\":13}] , [{\"name\":\"name21\",\"value\":21}, " + + "{\"name\":\"name22\",\"value\":22}],[{\"name\":\"name31\",\"value\":31, \"array\":[31,32,33], " + + "\"child\":{\"name\":\"name311\", \"array\":[31,32,33]}}]]"; + + @Test + void basicArrayStringOnlyTest() { + var parser = new JsonReader(jsonArrayStringOnly); + var document = parser.read(); + var testArray = Arrays.asList("one", "two", "three"); + + printDocument(document); + assertEquals(JsonDocument.Type.ARRAY, document.getType()); + assertEquals(testArray.size(), document.getArray().size()); + assertTrue(document.getArray().containsAll(testArray)); + } + + @Test + void basicArrayBooleanOnlyTest() { + JsonReader parser = new JsonReader(jsonArrayBooleanOnly); + JsonDocument document = parser.read(); + List testArray = Arrays.asList(true, false, false, false, true); + + assertEquals(JsonDocument.Type.ARRAY, document.getType()); + assertEquals(testArray.size(), document.getArray().size()); + assertTrue(document.getArray().containsAll(testArray)); + } + + @Test + void basicArrayIntegerOnlyTest() { + var parser = new JsonReader(jsonArrayIntegerOnly); + var document = parser.read(); + var testArray = Arrays.asList(1, 2, 3, 4, 5, 6, 7); + + printDocument(document); + assertEquals(JsonDocument.Type.ARRAY, document.getType()); + assertEquals(testArray.size(), document.getArray().size()); + assertTrue(document.getArray().containsAll(testArray)); + } + + @Test + void basicArrayNumberOnlyTest() { + var parser = new JsonReader(jsonArrayNumberOnly); + var document = parser.read(); + var testArray = Arrays.asList(0.2, 0.1, 0.3); + + assertEquals(JsonDocument.Type.ARRAY, document.getType()); + assertEquals(testArray.size(), document.getArray().size()); + assertTrue(document.getArray().containsAll(testArray)); + } + + @Test + void basicArrayObjectOnlyTest() { + + JsonDocument jsonDocumentChild1 = new JsonDocument(JsonDocument.Type.OBJECT); + jsonDocumentChild1.put("name", "name1"); + jsonDocumentChild1.put("value", 42); + + JsonDocument jsonDocumentChild2 = new JsonDocument(JsonDocument.Type.OBJECT); + jsonDocumentChild2.put("name", "name2"); + jsonDocumentChild2.put("value", 22); + + JsonDocument jsonDocumentChild3 = new JsonDocument(JsonDocument.Type.OBJECT); + jsonDocumentChild3.put("name", "name3"); + jsonDocumentChild3.put("value", 8); + + List expectedArrayResult = Arrays.asList(jsonDocumentChild1, jsonDocumentChild2, + jsonDocumentChild3); + + JsonReader parser = new JsonReader(jsonArrayObjectOnly); + JsonDocument document = parser.read(); + + printDocument(document); + assertEquals(JsonDocument.Type.ARRAY, document.getType()); + assertEquals(expectedArrayResult.size(), document.getArray().size()); + assertTrue(document.getArray().containsAll(expectedArrayResult)); + } + + @Test + void basicArrayOfArrayStringOnlyTest() { + + List stringArray1 = Arrays.asList("one", "two", "three"); + List stringArray2 = Arrays.asList("one", "two"); + List stringArray3 = Collections.singletonList("one"); + JsonReader parser = new JsonReader(jsonArrayOfArraysStringsOnly); + JsonDocument document = parser.read(); + + assertTrue(compareArrays(stringArray1, document, 0)); + assertTrue(compareArrays(stringArray2, document, 1)); + assertTrue(compareArrays(stringArray3, document, 2)); + printDocument(document); + } + + @Test + void basicArrayOfArraysIntegersOnlyTest() { + List stringArray1 = Arrays.asList(1, 2, 3); + List stringArray2 = Arrays.asList(1, 2); + List stringArray3 = Collections.singletonList(1); + + JsonReader parser = new JsonReader(jsonArrayOfArraysIntegersOnly); + JsonDocument document = parser.read(); + + assertTrue(compareArrays(stringArray1, document, 0)); + assertTrue(compareArrays(stringArray2, document, 1)); + assertTrue(compareArrays(stringArray3, document, 2)); + printDocument(document); + } + + @Test + void basicArrayOfArraysBooleanOnlyTest() { + List stringArray1 = Arrays.asList(true, false, true); + List stringArray2 = Arrays.asList(true, false); + List stringArray3 = Collections.singletonList(true); + + JsonReader parser = new JsonReader(jsonArrayOfArraysBooleansOnly); + JsonDocument document = parser.read(); + + assertTrue(compareArrays(stringArray1, document, 0)); + assertTrue(compareArrays(stringArray2, document, 1)); + assertTrue(compareArrays(stringArray3, document, 2)); + printDocument(document); + } + + @Test + void basicArrayOfArraysObjectsOnlyTest() { + List array1 = Arrays.asList(testObjectJsonDocument(11), testObjectJsonDocument(12), + testObjectJsonDocument(13)); + List array2 = Arrays.asList(testObjectJsonDocument(21), testObjectJsonDocument(22)); + List array3 = Collections.singletonList(testObjectJsonDocument(31)); + + JsonReader parser = new JsonReader(jsonArrayOfArraysObjectsOnly); + JsonDocument document = parser.read(); + + assertTrue(compareArrays(array1, document, 0)); + assertTrue(compareArrays(array2, document, 1)); + assertTrue(compareArrays(array3, document, 2)); + + printDocument(document); + } + + @Test + void basicArrayOfArraysObjectsWithObjectWithArrayTest() { + List array1 = Arrays.asList(testObjectJsonDocument(11), testObjectJsonDocument(12), + testObjectJsonDocument(13)); + List array2 = Arrays.asList(testObjectJsonDocument(21), testObjectJsonDocument(22)); + + JsonDocument lastJsonDocument = testObjectJsonDocument(31); + JsonDocument lastJsonDocumentArray = new JsonDocument(JsonDocument.Type.ARRAY); + lastJsonDocumentArray.add(31); + lastJsonDocumentArray.add(32); + lastJsonDocumentArray.add(33); + JsonDocument lastJsonDocumentObject = new JsonDocument(JsonDocument.Type.OBJECT); + lastJsonDocumentObject.put("name", "name311"); + lastJsonDocumentObject.put("array", lastJsonDocumentArray); + lastJsonDocument.put("array", lastJsonDocumentArray); + lastJsonDocument.put("child", lastJsonDocumentObject); + List array3 = Collections.singletonList(lastJsonDocument); + + JsonReader parser = new JsonReader(jsonArrayOfArraysObjectsWithObjectWithArray); + long start = System.nanoTime(); + JsonDocument document = parser.read(); + TimeUtils.printTimeDiffNano("robo4j:", start); + + assertTrue(compareArrays(array1, document, 0)); + assertTrue(compareArrays(array2, document, 1)); + assertTrue(compareArrays(array3, document, 2)); + + printDocument(document); + } + + private boolean compareArrays(List sourceArray, JsonDocument arrayDocument, int index) { + JsonDocument desiredArrayDocument = (JsonDocument) arrayDocument.getArray().get(index); + assertEquals(JsonDocument.Type.ARRAY, desiredArrayDocument.getType()); + return sourceArray.containsAll(desiredArrayDocument.getArray()); + } + + private JsonDocument testObjectJsonDocument(Integer value) { + JsonDocument result = new JsonDocument(JsonDocument.Type.OBJECT); + result.put("name", "name" + value); + result.put("value", value); + return result; + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderTests.java index 29da36e8..a80e478e 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonReaderTests.java @@ -19,320 +19,331 @@ import com.robo4j.socket.http.json.JsonDocument; import com.robo4j.socket.http.json.JsonReader; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.List; import java.util.Map; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class JsonReaderTests { + private static final Logger LOGGER = LoggerFactory.getLogger(JsonReaderTests.class); + + private static final String jsonBooleanValues = "{\"number\":22,\"message\":\"no message\",\"active\":true,\"passive\": false, \"bool1\":false,\"bool2\" :true}"; + private static final String jsonBasicValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : 0.42}"; + private static final String jsonBasicMinusValues = "{ \"number\"\n : -42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : -0.42}"; + private static final String jsonBasicValuesWithNullValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\"," + + " \"active\" : false , \"floatNumber\" : 0.42, \"empty1\":null, \"empty2\" : null }"; + private static final String jsonBasicValueWithStringArray = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " + + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}"; + private static final String jsonBasicValueWithStringAndIntegerArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " + + "\"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\", \"two\"], \"arrayTwo\" : [1, 2 ,3 ]}"; + private static final String jsonBasicValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " + + "\"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " + + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}], \"active\" : false}"; - private static final String jsonBooleanValues = "{\"number\":22,\"message\":\"no message\",\"active\":true,\"passive\": false, \"bool1\":false,\"bool2\" :true}"; - private static final String jsonBasicValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : 0.42}"; - private static final String jsonBasicMinusValues = "{ \"number\"\n : -42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : -0.42}"; - private static final String jsonBasicValuesWithNullValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\"," - + " \"active\" : false , \"floatNumber\" : 0.42, \"empty1\":null, \"empty2\" : null }"; - private static final String jsonBasicValueWithStringArray = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " - + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}"; - private static final String jsonBasicValueWithStringAndIntegerArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " - + "\"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\", \"two\"], \"arrayTwo\" : [1, 2 ,3 ]}"; - private static final String jsonBasicValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " - + "\"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " - + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}], \"active\" : false}"; - - private static final String jsonBasicValueAndObjectValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " - + "\"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " - + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}], \"active\" : false," - + "\"child\" :{\"name\":\"name11\",\"value\":1} }"; - - private static final String jsonArrayIntegerStringObject = "{\"arrayString\" : [\"one\", \"two\", \"three\"], \"arrayInteger\":[1,2,3]}"; - - private static final String jsonNestedObjects = "{\"value\" : { \"floatNumber\" : 0.42, \"object1\" : {\"name\" : \"some\"}}}"; - private static final String jsonNestedObjectAndArrayObject = "{\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]}"; - private static final String jsonNestedObjectWithBasicValueWithStringArray = "{\"value\" : { \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " - + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}}"; - - private static final String jsonNestedObject2BasicValueWithStringArray = "{\"name\" : \"nestedName1\", \"object1\" : {\"value\" : " - + "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}}}"; - - @Test - void basicBooleanValuesTest() { - - long start = System.nanoTime(); - JsonReader parser = new JsonReader(jsonBooleanValues); - JsonDocument document = parser.read(); - TimeUtils.printTimeDiffNano("basicBooleanValues robo4j", start); - - System.out.println("DOC: " + document); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(22, document.getKey("number")); - assertEquals("no message", document.getKey("message")); - assertEquals(true, document.getKey("active")); - assertEquals(false, document.getKey("passive")); - assertEquals(false, document.getKey("bool1")); - assertEquals(true, document.getKey("bool2")); - - } - - @Test - public void basicAndNullValues() { - JsonReader parser = new JsonReader(jsonBasicValuesWithNullValues); - JsonDocument document = parser.read(); - Map map = document.getMap(); + private static final String jsonBasicValueAndObjectValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " + + "\"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " + + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}], \"active\" : false," + + "\"child\" :{\"name\":\"name11\",\"value\":1} }"; + + private static final String jsonArrayIntegerStringObject = "{\"arrayString\" : [\"one\", \"two\", \"three\"], \"arrayInteger\":[1,2,3]}"; - System.out.println("DOC: " + document); - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals("no message", map.get("message")); - assertEquals(false, map.get("active")); - assertEquals(0.42, map.get("floatNumber")); - assertNull(map.get("empty1")); - assertNull(map.get("empty2")); - } - - @Test - void basicArrayIntegerStringTest() { - JsonReader parser = new JsonReader(jsonArrayIntegerStringObject); - JsonDocument document = parser.read(); - Map map = document.getMap(); - - List arrayString = ((JsonDocument) map.get("arrayString")).getArray(); - List arrayInteger = ((JsonDocument) map.get("arrayInteger")).getArray(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertTrue((Arrays.asList("one", "two", "three").containsAll(arrayString))); - assertTrue((Arrays.asList(1, 2, 3).containsAll(arrayInteger))); - System.out.println("DOC: " + document); - } - - @Test - void basicNestedObjectWithStringArray() { + private static final String jsonNestedObjects = "{\"value\" : { \"floatNumber\" : 0.42, \"object1\" : {\"name\" : \"some\"}}}"; + private static final String jsonNestedObjectAndArrayObject = "{\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]}"; + private static final String jsonNestedObjectWithBasicValueWithStringArray = "{\"value\" : { \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " + + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}}"; - JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument2.put("name", "some"); - - JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument1.put("floatNumber", 0.42); - nestedJsonDocument1.put("object1", nestedJsonDocument2); - - JsonReader parser = new JsonReader(jsonNestedObjects); - JsonDocument document = parser.read(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(nestedJsonDocument1, document.getKey("value")); - - System.out.println("document: " + document); - - } - - @Test - void basicNestedObjectValuesAndObjectArray() { - - JsonDocument nestedJsonDocument21 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument21.put("name", "name1"); - nestedJsonDocument21.put("value", 22); - - JsonDocument nestedJsonDocument22 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument22.put("name", "name2"); - nestedJsonDocument22.put("value", 42); - - JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.ARRAY); - nestedJsonDocument2.add(nestedJsonDocument21); - nestedJsonDocument2.add(nestedJsonDocument22); - - JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument1.put("arrayThree", nestedJsonDocument2); - - JsonReader parser = new JsonReader(jsonNestedObjectAndArrayObject); - JsonDocument document = parser.read(); - - JsonDocument arrayJsonDocument = (JsonDocument) document.getKey("arrayThree"); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(JsonDocument.Type.ARRAY, arrayJsonDocument.getType()); - assertEquals(nestedJsonDocument2, document.getKey("arrayThree")); - System.out.println("document: " + document); - } - - @Test - void basicNestedObjectWithBasicValueWithStringArray() { - - JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.ARRAY); - nestedJsonDocument2.add("one"); - nestedJsonDocument2.add("two"); - - JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument1.put("floatNumber", 0.42); - nestedJsonDocument1.put("number", 42); - nestedJsonDocument1.put("active", false); - nestedJsonDocument1.put("message", "no message"); - nestedJsonDocument1.put("arrayOne", nestedJsonDocument2); - - JsonReader parser = new JsonReader(jsonNestedObjectWithBasicValueWithStringArray); - JsonDocument document = parser.read(); - - JsonDocument nestedArray = ((JsonDocument) ((JsonDocument) document.getKey("value")).getKey("arrayOne")); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(nestedJsonDocument1, document.getKey("value")); - assertEquals(JsonDocument.Type.ARRAY, nestedArray.getType()); - System.out.println("document: " + document); - - } - - @Test - void basicNestedObject2WithBasicValueWithStringArray() { - - JsonDocument nestedJsonDocument4 = new JsonDocument(JsonDocument.Type.ARRAY); - nestedJsonDocument4.add("one"); - nestedJsonDocument4.add("two"); - - JsonDocument nestedJsonDocument3 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument3.put("floatNumber", 0.42); - nestedJsonDocument3.put("number", 42); - nestedJsonDocument3.put("active", false); - nestedJsonDocument3.put("message", "no message"); - nestedJsonDocument3.put("arrayOne", nestedJsonDocument4); - - JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument2.put("value", nestedJsonDocument3); - - JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); - nestedJsonDocument1.put("name", "nestedName1"); - nestedJsonDocument1.put("object1", nestedJsonDocument2); - - JsonReader parser = new JsonReader(jsonNestedObject2BasicValueWithStringArray); - JsonDocument document = parser.read(); - - JsonDocument expectedNestedObject2 = (JsonDocument) document.getKey("object1"); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals("nestedName1", document.getKey("name")); - assertEquals(expectedNestedObject2, nestedJsonDocument2); - assertEquals(JsonDocument.Type.OBJECT, expectedNestedObject2.getType()); - System.out.println("document: " + document); - } - - @Test - void basicValuesJsonParse() { - JsonReader parser = new JsonReader(jsonBasicValues); - JsonDocument document = parser.read(); - Map map = document.getMap(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals("no message", map.get("message")); - assertEquals(false, map.get("active")); - assertEquals(0.42, map.get("floatNumber")); - } - - @Test - void basicValuesMinusJsonParse() { - JsonReader parser = new JsonReader(jsonBasicMinusValues); - JsonDocument document = parser.read(); - Map map = document.getMap(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(-42, map.get("number")); - assertEquals("no message", map.get("message")); - assertEquals(false, map.get("active")); - assertEquals(-0.42, map.get("floatNumber")); - } - - @Test - void jsonBasicValuesAndStringArrayTest() { - JsonReader parser = new JsonReader(jsonBasicValueWithStringArray); - JsonDocument document = parser.read(); - Map map = document.getMap(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - List resultArray = ((JsonDocument) map.get("arrayOne")).getArray(); - assertTrue(Arrays.asList("one", "two").containsAll(resultArray)); - - System.out.println("document: " + document); - } - - @Test - void jsonBasicValuesAndStringAndIntegerArraysTest() { - JsonReader parser = new JsonReader(jsonBasicValueWithStringAndIntegerArrays); - JsonDocument document = parser.read(); - Map map = document.getMap(); - List resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); - List resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); - assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); - - System.out.println("document: " + document); - } - - @Test - void jsonBasicValuesAndStringAndIntegerAndObjectArraysTest() { - - JsonReader parser = new JsonReader(jsonBasicValueWithStringAndIntegerAndObjectArrays); - JsonDocument document = parser.read(); - System.out.println("document: " + document); - Map map = document.getMap(); - List resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); - List resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); - List resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); - JsonDocument arrayObj1 = (JsonDocument) resultObjectArray.get(0); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - assertEquals(2, resultObjectArray.size()); - assertEquals("name1", arrayObj1.getMap().get("name")); - assertEquals(22, arrayObj1.getMap().get("value")); - assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); - assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); - - } - - @Test - void jsonBasicValuesAndObjectValueAndStringAndIntegerAndObjectArraysTest() { - - JsonReader parser = new JsonReader(jsonBasicValueAndObjectValueWithStringAndIntegerAndObjectArrays); - JsonDocument document = parser.read(); - System.out.println("document: " + document); - Map map = document.getMap(); - List resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); - List resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); - List resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); - JsonDocument arrayObj1 = (JsonDocument) resultObjectArray.get(0); - JsonDocument childObj = (JsonDocument) map.get("child"); - - assertEquals(JsonDocument.Type.OBJECT, document.getType()); - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - assertEquals(2, resultObjectArray.size()); - assertEquals("name1", arrayObj1.getMap().get("name")); - assertEquals(22, arrayObj1.getMap().get("value")); - assertEquals("name11", childObj.getMap().get("name")); - assertEquals(1, childObj.getMap().get("value")); - assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); - assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); - - } + private static final String jsonNestedObject2BasicValueWithStringArray = "{\"name\" : \"nestedName1\", \"object1\" : {\"value\" : " + + "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}}}"; + + @Test + void basicBooleanValuesTest() { + var start = System.nanoTime(); + var parser = new JsonReader(jsonBooleanValues); + var document = parser.read(); + TimeUtils.printTimeDiffNano("basicBooleanValues robo4j", start); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(22, document.getKey("number")); + assertEquals("no message", document.getKey("message")); + assertEquals(true, document.getKey("active")); + assertEquals(false, document.getKey("passive")); + assertEquals(false, document.getKey("bool1")); + assertEquals(true, document.getKey("bool2")); + + } + + @Test + public void basicAndNullValues() { + var parser = new JsonReader(jsonBasicValuesWithNullValues); + var document = parser.read(); + var map = document.getMap(); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals("no message", map.get("message")); + assertEquals(false, map.get("active")); + assertEquals(0.42, map.get("floatNumber")); + assertNull(map.get("empty1")); + assertNull(map.get("empty2")); + } + + @Test + void basicArrayIntegerStringTest() { + JsonReader parser = new JsonReader(jsonArrayIntegerStringObject); + JsonDocument document = parser.read(); + Map map = document.getMap(); + + List arrayString = ((JsonDocument) map.get("arrayString")).getArray(); + List arrayInteger = ((JsonDocument) map.get("arrayInteger")).getArray(); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertTrue((Arrays.asList("one", "two", "three").containsAll(arrayString))); + assertTrue((Arrays.asList(1, 2, 3).containsAll(arrayInteger))); + } + + @Test + void basicNestedObjectWithStringArray() { + + JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument2.put("name", "some"); + + JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument1.put("floatNumber", 0.42); + nestedJsonDocument1.put("object1", nestedJsonDocument2); + + JsonReader parser = new JsonReader(jsonNestedObjects); + JsonDocument document = parser.read(); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(nestedJsonDocument1, document.getKey("value")); + } + + @Test + void basicNestedObjectValuesAndObjectArray() { + + JsonDocument nestedJsonDocument2 = getNestedDocument2(); + + JsonReader parser = new JsonReader(jsonNestedObjectAndArrayObject); + JsonDocument document = parser.read(); + + JsonDocument arrayJsonDocument = (JsonDocument) document.getKey("arrayThree"); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(JsonDocument.Type.ARRAY, arrayJsonDocument.getType()); + assertEquals(nestedJsonDocument2, document.getKey("arrayThree")); + } + + @Test + void basicNestedObjectWithBasicValueWithStringArray() { + var nestedJsonDocument1 = getNestedDocument(); + + var parser = new JsonReader(jsonNestedObjectWithBasicValueWithStringArray); + var document = parser.read(); + var nestedArray = ((JsonDocument) ((JsonDocument) document.getKey("value")).getKey("arrayOne")); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(nestedJsonDocument1, document.getKey("value")); + assertEquals(JsonDocument.Type.ARRAY, nestedArray.getType()); + } + + private static JsonDocument getNestedDocument() { + var nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.ARRAY); + nestedJsonDocument2.add("one"); + nestedJsonDocument2.add("two"); + + var nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument1.put("floatNumber", 0.42); + nestedJsonDocument1.put("number", 42); + nestedJsonDocument1.put("active", false); + nestedJsonDocument1.put("message", "no message"); + nestedJsonDocument1.put("arrayOne", nestedJsonDocument2); + return nestedJsonDocument1; + } + + private static JsonDocument getNestedDocument2() { + JsonDocument nestedJsonDocument21 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument21.put("name", "name1"); + nestedJsonDocument21.put("value", 22); + + JsonDocument nestedJsonDocument22 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument22.put("name", "name2"); + nestedJsonDocument22.put("value", 42); + + JsonDocument nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.ARRAY); + nestedJsonDocument2.add(nestedJsonDocument21); + nestedJsonDocument2.add(nestedJsonDocument22); + + JsonDocument nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument1.put("arrayThree", nestedJsonDocument2); + return nestedJsonDocument2; + } + + @Test + void basicNestedObject2WithBasicValueWithStringArray() { + var nestedJsonDocument2 = getJsonDocument(); + var nestedJsonDocument1 = new JsonDocument(JsonDocument.Type.OBJECT); + + nestedJsonDocument1.put("name", "nestedName1"); + nestedJsonDocument1.put("object1", nestedJsonDocument2); + + var parser = new JsonReader(jsonNestedObject2BasicValueWithStringArray); + var document = parser.read(); + var expectedNestedObject2 = (JsonDocument) document.getKey("object1"); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals("nestedName1", document.getKey("name")); + assertEquals(expectedNestedObject2, nestedJsonDocument2); + assertEquals(JsonDocument.Type.OBJECT, expectedNestedObject2.getType()); + } + + @Test + void basicValuesJsonParse() { + var parser = new JsonReader(jsonBasicValues); + var document = parser.read(); + var map = document.getMap(); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals("no message", map.get("message")); + assertEquals(false, map.get("active")); + assertEquals(0.42, map.get("floatNumber")); + } + + @Test + void basicValuesMinusJsonParse() { + var parser = new JsonReader(jsonBasicMinusValues); + var document = parser.read(); + var map = document.getMap(); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(-42, map.get("number")); + assertEquals("no message", map.get("message")); + assertEquals(false, map.get("active")); + assertEquals(-0.42, map.get("floatNumber")); + } + + @Test + void jsonBasicValuesAndStringArrayTest() { + var parser = new JsonReader(jsonBasicValueWithStringArray); + var document = parser.read(); + var map = document.getMap(); + var resultArray = ((JsonDocument) map.get("arrayOne")).getArray(); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertTrue(Arrays.asList("one", "two").containsAll(resultArray)); + } + + @Test + void jsonBasicValuesAndStringAndIntegerArraysTest() { + var parser = new JsonReader(jsonBasicValueWithStringAndIntegerArrays); + var document = parser.read(); + var map = document.getMap(); + var resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); + var resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); + assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); + } + + @Test + void jsonBasicValuesAndStringAndIntegerAndObjectArraysTest() { + var parser = new JsonReader(jsonBasicValueWithStringAndIntegerAndObjectArrays); + var document = parser.read(); + var map = document.getMap(); + var resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); + var resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); + var resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); + var arrayObj1 = (JsonDocument) resultObjectArray.get(0); + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertEquals(2, resultObjectArray.size()); + assertEquals("name1", arrayObj1.getMap().get("name")); + assertEquals(22, arrayObj1.getMap().get("value")); + assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); + assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); + + } + + @Test + void jsonBasicValuesAndObjectValueAndStringAndIntegerAndObjectArraysTest() { + + var parser = new JsonReader(jsonBasicValueAndObjectValueWithStringAndIntegerAndObjectArrays); + var document = parser.read(); + var map = document.getMap(); + var resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); + var resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); + var resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); + JsonDocument arrayObj1 = (JsonDocument) resultObjectArray.get(0); + JsonDocument childObj = (JsonDocument) map.get("child"); + + + printDocument(document); + + assertEquals(JsonDocument.Type.OBJECT, document.getType()); + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertEquals(2, resultObjectArray.size()); + assertEquals("name1", arrayObj1.getMap().get("name")); + assertEquals(22, arrayObj1.getMap().get("value")); + assertEquals("name11", childObj.getMap().get("name")); + assertEquals(1, childObj.getMap().get("value")); + assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); + assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); + + } + + private static JsonDocument getJsonDocument() { + var nestedJsonDocument3 = getNestedDocument(); + + var nestedJsonDocument2 = new JsonDocument(JsonDocument.Type.OBJECT); + nestedJsonDocument2.put("value", nestedJsonDocument3); + return nestedJsonDocument2; + } + + static void printDocument(JsonDocument document) { + LOGGER.debug("DOC:{}", document); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonTest.java index 04b2aaaa..db529e8c 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonTest.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonTest.java @@ -19,11 +19,13 @@ import com.robo4j.socket.http.json.JsonDocument; import com.robo4j.socket.http.json.JsonReader; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; -import java.util.List; import java.util.Map; +import static com.robo4j.socket.http.test.json.JsonReaderTests.printDocument; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -32,132 +34,135 @@ * @author Miroslav Wengner (@miragemiko) */ class JsonTest { - - private static final String jsonBasicValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : 0.42}"; - private static final String jsonBasicValueWithStringArray = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " - + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}"; - private static final String jsonBasicValueWithStringAndIntegerArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " - + "\"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\", \"two\"], \"arrayTwo\" : [1, 2 ,3 ]}"; - private static final String jsonBasicValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " - + "\"active\" : false, \"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " - + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]}"; - - private static final String jsonBasicObjectArrays = "{\"number\"\n : 42,\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]," - + " \"active\" : false}"; - - private static final String jsonBasicObjectArraysAndStringMap = "{\"number\"\n : 42," - + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]," - + " \"active\" : false, \"simpleMap\": {\"one\":\"one1\",\"two\":\"two2\"}}"; - - @Test - void basicValuesJsonParse() { - JsonReader parser = new JsonReader(jsonBasicValues); - JsonDocument document = parser.read(); - Map map = document.getMap(); - assertEquals(42, map.get("number")); - assertEquals("no message", map.get("message")); - assertEquals(false, map.get("active")); - assertEquals(0.42, map.get("floatNumber")); - } - - @Test - void jsonBasicValuesAndStringArrayTest() { - JsonReader parser = new JsonReader(jsonBasicValueWithStringArray); - JsonDocument document = parser.read(); - Map map = document.getMap(); - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - List resultArray = ((JsonDocument) map.get("arrayOne")).getArray(); - assertTrue(Arrays.asList("one", "two").containsAll(resultArray)); - - System.out.println("document: " + document); - } - - @Test - void jsonBasicValuesAndStringAndIntegerArraysTest() { - JsonReader parser = new JsonReader(jsonBasicValueWithStringAndIntegerArrays); - JsonDocument document = parser.read(); - Map map = document.getMap(); - List resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); - List resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); - - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); - assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); - - System.out.println("document: " + document); - } - - @Test - void jsonBasicValuesAndStringAndIntegerAndObjectArraysTest() { - JsonReader parser = new JsonReader(jsonBasicValueWithStringAndIntegerAndObjectArrays); - JsonDocument document = parser.read(); - System.out.println("document: " + document); - Map map = document.getMap(); - List resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); - List resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); - - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("no message", map.get("message")); - assertEquals(0.42, map.get("floatNumber")); - assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); - assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); - - } - - @Test - void jsonBasicObjectArraysTest() { - - JsonReader parser = new JsonReader(jsonBasicObjectArrays); - JsonDocument document = parser.read(); - System.out.println("document: " + document); - Map map = document.getMap(); - List resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); - - JsonDocument obj1 = (JsonDocument) resultObjectArray.get(0); - JsonDocument obj2 = (JsonDocument) resultObjectArray.get(1); - - // [{"name":"name1","value": 22}, {"name":"name2","value": 42}] - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("name1", obj1.getMap().get("name").toString()); - assertEquals(22, (int) obj1.getMap().get("value")); - assertEquals("name2", obj2.getMap().get("name").toString()); - assertEquals(42, (int) obj2.getMap().get("value")); - assertEquals(2, resultObjectArray.size()); - - } - - @Test - void jsonBasicObjectArraysAndSimpleMapTest() { - - JsonReader parser = new JsonReader(jsonBasicObjectArraysAndStringMap); - JsonDocument document = parser.read(); - System.out.println("document: " + document); - Map map = document.getMap(); - List resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); - Map simpleMap = ((JsonDocument) map.get("simpleMap")).getMap(); - - JsonDocument obj1 = (JsonDocument) resultObjectArray.get(0); - JsonDocument obj2 = (JsonDocument) resultObjectArray.get(1); - - // [{"name":"name1","value": 22}, {"name":"name2","value": 42}] - assertEquals(42, map.get("number")); - assertEquals(false, map.get("active")); - assertEquals("name1", obj1.getMap().get("name").toString()); - assertEquals(22, (int) obj1.getMap().get("value")); - assertEquals("name2", obj2.getMap().get("name").toString()); - assertEquals(42, (int) obj2.getMap().get("value")); - assertEquals(2, resultObjectArray.size()); - assertEquals("one1", simpleMap.get("one")); - assertEquals("two2", simpleMap.get("two")); - - } + private static final Logger LOGGER = LoggerFactory.getLogger(JsonTest.class); + + private static final String jsonBasicValues = "{ \"number\"\n : 42, \"message\" \t: \"no message\", \"active\" : false , \"floatNumber\" : 0.42}"; + private static final String jsonBasicValueWithStringArray = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, \"active\" : false, " + + "\"message\" \t: \"no message\", \"arrayOne\":[\"one\",\"two\"]}"; + private static final String jsonBasicValueWithStringAndIntegerArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " + + "\"active\" : false, \"message\" \t: \"no message\", \"arrayOne\":[\"one\", \"two\"], \"arrayTwo\" : [1, 2 ,3 ]}"; + private static final String jsonBasicValueWithStringAndIntegerAndObjectArrays = "{ \"floatNumber\" : 0.42, \"number\"\n : 42, " + + "\"active\" : false, \"arrayOne\":[\"one\", \"two\"], \"message\" \t: \"no message\", \"arrayTwo\" : [1, 2 ,3 ], " + + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]}"; + + private static final String jsonBasicObjectArrays = "{\"number\"\n : 42,\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]," + + " \"active\" : false}"; + + private static final String jsonBasicObjectArraysAndStringMap = "{\"number\"\n : 42," + + "\"arrayThree\" : [{\"name\":\"name1\",\"value\": 22}, {\"name\":\"name2\",\"value\": 42}]," + + " \"active\" : false, \"simpleMap\": {\"one\":\"one1\",\"two\":\"two2\"}}"; + + @Test + void basicValuesJsonParse() { + JsonReader parser = new JsonReader(jsonBasicValues); + JsonDocument document = parser.read(); + Map map = document.getMap(); + assertEquals(42, map.get("number")); + assertEquals("no message", map.get("message")); + assertEquals(false, map.get("active")); + assertEquals(0.42, map.get("floatNumber")); + } + + @Test + void jsonBasicValuesAndStringArrayTest() { + var parser = new JsonReader(jsonBasicValueWithStringArray); + var document = parser.read(); + var map = document.getMap(); + var resultArray = ((JsonDocument) map.get("arrayOne")).getArray(); + + printDocument(document); + + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertTrue(Arrays.asList("one", "two").containsAll(resultArray)); + + } + + @Test + void jsonBasicValuesAndStringAndIntegerArraysTest() { + var parser = new JsonReader(jsonBasicValueWithStringAndIntegerArrays); + var document = parser.read(); + var map = document.getMap(); + var resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); + var resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); + + printDocument(document); + + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); + assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); + } + + @Test + void jsonBasicValuesAndStringAndIntegerAndObjectArraysTest() { + var parser = new JsonReader(jsonBasicValueWithStringAndIntegerAndObjectArrays); + var document = parser.read(); + var map = document.getMap(); + var resultStringArray = ((JsonDocument) map.get("arrayOne")).getArray(); + var resultIntegerArray = ((JsonDocument) map.get("arrayTwo")).getArray(); + + printDocument(document); + + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("no message", map.get("message")); + assertEquals(0.42, map.get("floatNumber")); + assertTrue(Arrays.asList("one", "two").containsAll(resultStringArray)); + assertTrue(Arrays.asList(1, 2, 3).containsAll(resultIntegerArray)); + + } + + @Test + void jsonBasicObjectArraysTest() { + var parser = new JsonReader(jsonBasicObjectArrays); + var document = parser.read(); + var map = document.getMap(); + var resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); + + JsonDocument obj1 = (JsonDocument) resultObjectArray.get(0); + JsonDocument obj2 = (JsonDocument) resultObjectArray.get(1); + + printDocument(document); + + // [{"name":"name1","value": 22}, {"name":"name2","value": 42}] + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("name1", obj1.getMap().get("name").toString()); + assertEquals(22, (int) obj1.getMap().get("value")); + assertEquals("name2", obj2.getMap().get("name").toString()); + assertEquals(42, (int) obj2.getMap().get("value")); + assertEquals(2, resultObjectArray.size()); + } + + @Test + void jsonBasicObjectArraysAndSimpleMapTest() { + var parser = new JsonReader(jsonBasicObjectArraysAndStringMap); + var document = parser.read(); + var map = document.getMap(); + var resultObjectArray = ((JsonDocument) map.get("arrayThree")).getArray(); + var simpleMap = ((JsonDocument) map.get("simpleMap")).getMap(); + + JsonDocument obj1 = (JsonDocument) resultObjectArray.get(0); + JsonDocument obj2 = (JsonDocument) resultObjectArray.get(1); + + printDocument(document); + + // [{"name":"name1","value": 22}, {"name":"name2","value": 42}] + assertEquals(42, map.get("number")); + assertEquals(false, map.get("active")); + assertEquals("name1", obj1.getMap().get("name").toString()); + assertEquals(22, (int) obj1.getMap().get("value")); + assertEquals("name2", obj2.getMap().get("name").toString()); + assertEquals(42, (int) obj2.getMap().get("value")); + assertEquals(2, resultObjectArray.size()); + assertEquals("one1", simpleMap.get("one")); + assertEquals("two2", simpleMap.get("two")); + + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/TimeUtils.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/TimeUtils.java index 2725b5dc..51fe7da8 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/TimeUtils.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/TimeUtils.java @@ -16,14 +16,17 @@ */ package com.robo4j.socket.http.test.json; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class TimeUtils { + private static final Logger LOGGER = LoggerFactory.getLogger(TimeUtils.class); - static void printTimeDiffNano(String message, long start) { - - System.out.println(String.format("message: %s duration: %d", message, System.nanoTime() - start)); + static void printTimeDiffNano(String message, long start) { + LOGGER.debug("message: {} duration: {}", message, System.nanoTime() - start); } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/message/DatagramDecoratedRequestTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/message/DatagramDecoratedRequestTest.java index 9125ec48..919f2b21 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/message/DatagramDecoratedRequestTest.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/message/DatagramDecoratedRequestTest.java @@ -20,34 +20,36 @@ import com.robo4j.socket.http.message.DatagramDenominator; import com.robo4j.socket.http.util.DatagramBodyType; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static org.junit.jupiter.api.Assertions.assertNotNull; import static org.junit.jupiter.api.Assertions.assertTrue; /** - * @see DatagramDecoratedRequest - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) + * @see DatagramDecoratedRequest */ class DatagramDecoratedRequestTest { + private static final Logger LOGGER = LoggerFactory.getLogger(DatagramDecoratedRequestTest.class); - @Test - void datagramDecoratedRequestTest() { + @Test + void datagramDecoratedRequestTest() { - DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), - "/units/stringConsumer"); - DatagramDecoratedRequest request = new DatagramDecoratedRequest(denominator); - request.addMessage("{\"number\":22}".getBytes()); + DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), + "/units/stringConsumer"); + DatagramDecoratedRequest request = new DatagramDecoratedRequest(denominator); + request.addMessage("{\"number\":22}".getBytes()); - byte[] requestBytes = request.toMessage(); + byte[] requestBytes = request.toMessage(); - String requestMessage = new String(requestBytes); + String requestMessage = new String(requestBytes); - System.out.println("requestMessage: " + requestMessage); + LOGGER.debug("requestMessage:{}", requestMessage); - assertNotNull(requestBytes); - assertTrue(requestBytes.length > 0); + assertNotNull(requestBytes); + assertTrue(requestBytes.length > 0); - } + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/request/ByteBufferTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/request/ByteBufferTests.java index d8cabd64..91ed4e86 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/request/ByteBufferTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/request/ByteBufferTests.java @@ -27,116 +27,115 @@ import com.robo4j.socket.http.util.HttpMessageBuilder; import com.robo4j.socket.http.util.RoboHttpUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.nio.ByteBuffer; import java.util.regex.Matcher; import java.util.regex.Pattern; import static com.robo4j.socket.http.HttpHeaderFieldValues.CONNECTION_KEEP_ALIVE; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ class ByteBufferTests { - - private static final String TEST_STRING = "Accept-Language: en-US,en;q=0.8\r\n\r\n{"; - private static final String TEST_POSTMAN_MESSAGE = "\r\n" + "{ \n" + " \"value\" : \"move\"\n" + "}"; - private static final String TEST_POSTMAN_STRING = "POST /controller HTTP/1.1\r\n" + "Host: localhost:8042\r\n" - + "Connection: " + CONNECTION_KEEP_ALIVE + "\r\n" + "Content-Length: 23\r\n" - + "Postman-Token: 60b492c5-e7a9-6037-3021-42f8885542a9\r\n" + "Cache-Control: no-cache\r\n" - + "Origin: chrome-extension://fhbjgbiflinjbdggehcddcbncdddomop\r\n" - + "User-Agent: Mozilla/5.0 (Macintosh; Intel Mac OS X 10_12_6) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/60.0.3112.101 Safari/537.36\r\n" - + "Content-Type: text/plain;charset=UTF-8\r\n" + "Accept: */*\r\n" - + "Accept-Encoding: gzip, deflate, br\r\n" + "Accept-Language: en-US,en;q=0.8\r\n" + TEST_POSTMAN_MESSAGE; - private static final ByteBuffer TEST_BYTE_BUFFER = ByteBuffer.wrap(TEST_STRING.getBytes()); - - @Test - void testPostmanMessage() { - HttpDecoratedRequest decoratedRequest = ChannelBufferUtils - .extractDecoratedRequestByStringMessage(TEST_POSTMAN_STRING); - decoratedRequest.addMessage(TEST_POSTMAN_MESSAGE); - - assertNotNull(decoratedRequest.getHeader()); - assertTrue(!decoratedRequest.getHeader().isEmpty()); - assertNotNull(decoratedRequest.getMessage()); - System.out.println("HEADER: " + decoratedRequest.getHeader()); - System.out.println("BODY: " + decoratedRequest.getMessage()); - - } - - @Test - void byteBufferFromRequestTest() { - - String bodyMessage = "this is test message"; - String host = "localhost"; - String clientPath = "/test"; - - HttpDenominator denominator = new HttpRequestDenominator(HttpMethod.POST, clientPath, HttpVersion.HTTP_1_1); - String postMessage = HttpMessageBuilder.Build().setDenominator(denominator) - .addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(bodyMessage.length())) - .addHeaderElement(HttpHeaderFieldNames.HOST, - RoboHttpUtils.createHost(host, ProtocolType.HTTP.getPort())) - .build(bodyMessage); - - HttpDecoratedRequest decoratedRequest = ChannelBufferUtils.extractDecoratedRequestByStringMessage(postMessage); - - assertNotNull(postMessage); - assertEquals(postMessage.length(), decoratedRequest.getLength()); - assertEquals(clientPath, decoratedRequest.getPathMethod().getPath()); - assertEquals(bodyMessage, decoratedRequest.getMessage()); - } - - @Test - void testMovingWindow() { - String correctString = "\n\n"; - assertTrue(ChannelBufferUtils.isBWindow(ChannelBufferUtils.END_WINDOW, - ByteBuffer.wrap(correctString.getBytes()).array())); - } - - @Test - void testReturnCharRemoval() { - - int position = 0; - int bPosition = 0; - int size = TEST_BYTE_BUFFER.capacity(); - byte[] tmpBytes = new byte[1024]; - - while (position < size) { - byte b = TEST_BYTE_BUFFER.get(position); - if (b == ChannelBufferUtils.CHAR_RETURN) { - } else { - tmpBytes[bPosition] = b; - bPosition++; - } - position++; - } - - byte[] resBytes = ChannelBufferUtils.validArray(tmpBytes, bPosition); - ByteBuffer resultBuffer = ByteBuffer.wrap(resBytes); - assertEquals(TEST_BYTE_BUFFER.capacity(), resultBuffer.capacity() + ChannelBufferUtils.END_WINDOW.length); - } - - @Test - void test() { - Pattern pattern = Pattern.compile("^(.*)[\\r\\n{2}]|[\\n\\n](.*)$"); - - String one = "some\r\n\r\nother"; - String two = "some\n\nother"; - - Matcher matcher1 = pattern.matcher(one); - Matcher matcher2 = pattern.matcher(two); - - // Assert.assertTrue(matcher1.find()); - // Assert.assertTrue(matcher2.find()); - // Assert.assertTrue(matcher2.groupCount() == matcher1.groupCount()); - // Assert.assertTrue(matcher2.groupCount() == matcher1.groupCount()); - System.out.println("ONE: " + matcher1.find() + " group: " + matcher1.groupCount() + " 1: " + matcher1.group(1) - + " 2: " + matcher1.group(0)); - System.out.println("TWO: " + matcher2.find() + " group: " + matcher2.groupCount()); - - } + private static final Logger LOGGER = LoggerFactory.getLogger(ByteBufferTests.class); + private static final String TEST_STRING = "Accept-Language: en-US,en;q=0.8\r\n\r\n{"; + private static final String TEST_POSTMAN_MESSAGE = "\r\n" + "{ \n" + " \"value\" : \"move\"\n" + "}"; + private static final String TEST_POSTMAN_STRING = "POST /controller HTTP/1.1\r\n" + "Host: localhost:8042\r\n" + + "Connection: " + CONNECTION_KEEP_ALIVE + "\r\n" + "Content-Length: 23\r\n" + + "Postman-Token: 60b492c5-e7a9-6037-3021-42f8885542a9\r\n" + "Cache-Control: no-cache\r\n" + + "Origin: chrome-extension://fhbjgbiflinjbdggehcddcbncdddomop\r\n" + + "User-Agent: Mozilla/5.0 (Macintosh; Intel Mac OS X 10_12_6) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/60.0.3112.101 Safari/537.36\r\n" + + "Content-Type: text/plain;charset=UTF-8\r\n" + "Accept: */*\r\n" + + "Accept-Encoding: gzip, deflate, br\r\n" + "Accept-Language: en-US,en;q=0.8\r\n" + TEST_POSTMAN_MESSAGE; + private static final ByteBuffer TEST_BYTE_BUFFER = ByteBuffer.wrap(TEST_STRING.getBytes()); + + @Test + void testPostmanMessage() { + HttpDecoratedRequest decoratedRequest = ChannelBufferUtils + .extractDecoratedRequestByStringMessage(TEST_POSTMAN_STRING); + decoratedRequest.addMessage(TEST_POSTMAN_MESSAGE); + + assertNotNull(decoratedRequest.getHeader()); + assertFalse(decoratedRequest.getHeader().isEmpty()); + assertNotNull(decoratedRequest.getMessage()); + LOGGER.debug("HEADER:{}", decoratedRequest.getHeader()); + LOGGER.debug("BODY:{}", decoratedRequest.getMessage()); + + } + + @Test + void byteBufferFromRequestTest() { + + String bodyMessage = "this is test message"; + String host = "localhost"; + String clientPath = "/test"; + + HttpDenominator denominator = new HttpRequestDenominator(HttpMethod.POST, clientPath, HttpVersion.HTTP_1_1); + String postMessage = HttpMessageBuilder.Build().setDenominator(denominator) + .addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(bodyMessage.length())) + .addHeaderElement(HttpHeaderFieldNames.HOST, + RoboHttpUtils.createHost(host, ProtocolType.HTTP.getPort())) + .build(bodyMessage); + + HttpDecoratedRequest decoratedRequest = ChannelBufferUtils.extractDecoratedRequestByStringMessage(postMessage); + + assertNotNull(postMessage); + assertEquals(postMessage.length(), decoratedRequest.getLength()); + assertEquals(clientPath, decoratedRequest.getPathMethod().getPath()); + assertEquals(bodyMessage, decoratedRequest.getMessage()); + } + + @Test + void testMovingWindow() { + String correctString = "\n\n"; + assertTrue(ChannelBufferUtils.isBWindow(ChannelBufferUtils.END_WINDOW, + ByteBuffer.wrap(correctString.getBytes()).array())); + } + + @Test + void testReturnCharRemoval() { + + int position = 0; + int bPosition = 0; + int size = TEST_BYTE_BUFFER.capacity(); + byte[] tmpBytes = new byte[1024]; + + while (position < size) { + byte b = TEST_BYTE_BUFFER.get(position); + if (b == ChannelBufferUtils.CHAR_RETURN) { + } else { + tmpBytes[bPosition] = b; + bPosition++; + } + position++; + } + + byte[] resBytes = ChannelBufferUtils.validArray(tmpBytes, bPosition); + ByteBuffer resultBuffer = ByteBuffer.wrap(resBytes); + assertEquals(TEST_BYTE_BUFFER.capacity(), resultBuffer.capacity() + ChannelBufferUtils.END_WINDOW.length); + } + + @Test + void test() { + Pattern pattern = Pattern.compile("^(.*)[\\r\\n{2}]|[\\n\\n](.*)$"); + + String one = "some\r\n\r\nother"; + String two = "some\n\nother"; + + Matcher matcher1 = pattern.matcher(one); + Matcher matcher2 = pattern.matcher(two); + + // Assert.assertTrue(matcher1.find()); + // Assert.assertTrue(matcher2.find()); + // Assert.assertTrue(matcher2.groupCount() == matcher1.groupCount()); + // Assert.assertTrue(matcher2.groupCount() == matcher1.groupCount()); + LOGGER.debug("ONE: {} group: {} 1: {} 2: {}", matcher1.find(), matcher1.groupCount(), matcher1.group(1), matcher1.group(0)); + LOGGER.debug("TWO: {} group: {}", matcher2.find(), matcher2.groupCount()); + + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpContextTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpContextTests.java index 7d8a9095..7a32d36b 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpContextTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpContextTests.java @@ -27,78 +27,74 @@ import com.robo4j.util.StringConstants; import com.robo4j.util.Utf8Constant; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.io.InputStream; import java.util.Collections; -import java.util.List; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** - * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class HttpContextTests { - - @Test - void serverNotInitiatedContextTest() { - - ServerContext context = new ServerContext(); - System.out.println("context:" + context); - assertNotNull(context); - assertTrue(context.isEmpty()); - - } - - @Test - void serverDefaultContextTest() { - - ServerContext context = new ServerContext(); - HttpPathUtils.updateHttpServerContextPaths(null, context, Collections.emptyList()); - - System.out.println("context:" + context); - assertNotNull(context); - assertTrue(!context.isEmpty()); - assertTrue(context.containsPath(new PathHttpMethod(Utf8Constant.UTF8_SOLIDUS, HttpMethod.GET))); - - } - - @Test - void clientDefaultContextTest() { - - ClientContext context = new ClientContext(); - - System.out.println("context: " + context); - assertNotNull(context); - assertTrue(context.isEmpty()); - } - - @Test - void clientSimpleContextTest() throws Exception { - - RoboBuilder builderProducer = new RoboBuilder(); - InputStream contextIS = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("robo_client_context.xml"); - builderProducer.add(contextIS); - - List paths = Collections.singletonList(new HttpPathMethodDTO(StringConstants.EMPTY, - HttpMethod.GET, Collections.singletonList(StringConsumer.NAME))); - - ClientContext context = new ClientContext(); - HttpPathUtils.updateHttpClientContextPaths(context, paths); - - PathHttpMethod basicGet = new PathHttpMethod(Utf8Constant.UTF8_SOLIDUS, HttpMethod.GET); - - System.out.println("context: " + context); - assertNotNull(context); - assertNotNull(context.getPathConfig(basicGet)); - assertTrue(!context.getPathConfig(basicGet).getCallbacks().isEmpty()); - assertEquals(HttpMethod.GET, context.getPathConfig(basicGet).getMethod()); - assertEquals(1, context.getPathConfig(basicGet).getCallbacks().size()); - assertEquals(StringConsumer.NAME, context.getPathConfig(basicGet).getCallbacks().get(0)); - } + private static final Logger LOGGER = LoggerFactory.getLogger(HttpContextTests.class); + + @Test + void serverNotInitiatedContextTest() { + ServerContext context = new ServerContext(); + + printContext(context); + assertNotNull(context); + assertTrue(context.isEmpty()); + + } + + @Test + void serverDefaultContextTest() { + var context = new ServerContext(); + HttpPathUtils.updateHttpServerContextPaths(null, context, Collections.emptyList()); + + printContext(context); + assertNotNull(context); + assertFalse(context.isEmpty()); + assertTrue(context.containsPath(new PathHttpMethod(Utf8Constant.UTF8_SOLIDUS, HttpMethod.GET))); + + } + + @Test + void clientDefaultContextTest() { + var context = new ClientContext(); + + printContext(context); + assertNotNull(context); + assertTrue(context.isEmpty()); + } + + @Test + void clientSimpleContextTest() throws Exception { + var builderProducer = new RoboBuilder(); + var contextIS = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("robo_client_context.xml"); + builderProducer.add(contextIS); + var paths = Collections.singletonList(new HttpPathMethodDTO(StringConstants.EMPTY, + HttpMethod.GET, Collections.singletonList(StringConsumer.NAME))); + var context = new ClientContext(); + HttpPathUtils.updateHttpClientContextPaths(context, paths); + + var basicGet = new PathHttpMethod(Utf8Constant.UTF8_SOLIDUS, HttpMethod.GET); + + printContext(context); + assertNotNull(context); + assertNotNull(context.getPathConfig(basicGet)); + assertFalse(context.getPathConfig(basicGet).getCallbacks().isEmpty()); + assertEquals(HttpMethod.GET, context.getPathConfig(basicGet).getMethod()); + assertEquals(1, context.getPathConfig(basicGet).getCallbacks().size()); + assertEquals(StringConsumer.NAME, context.getPathConfig(basicGet).getCallbacks().get(0)); + } + + private static void printContext(T context) { + LOGGER.debug("context:{}", context); + } } diff --git a/robo4j-socket-http/src/test/java/module-info.java b/robo4j-socket-http/src/test/java/module-info.java index ac340134..c8fb55d9 100644 --- a/robo4j-socket-http/src/test/java/module-info.java +++ b/robo4j-socket-http/src/test/java/module-info.java @@ -4,6 +4,7 @@ requires org.junit.jupiter.api; requires org.junit.jupiter.engine; + requires org.slf4j; exports com.robo4j.socket.http.test.utils; From 7928ac7cfd6df9ca3fb954451e4286e2db68671c Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Tue, 8 Oct 2024 11:34:00 +0200 Subject: [PATCH 06/13] [69] robo4j-socket-http next chunk --- robo4j-hw-rpi/pom.xml | 17 +- robo4j-socket-http/pom.xml | 10 +- .../http/test/json/JsonCodecsTests.java | 53 ++-- .../test/units/HttpServerConfigTests.java | 152 ++++++----- .../http/test/units/HttpServerUnitTests.java | 73 +++--- .../test/units/RoboDatagramClientTest.java | 14 +- .../test/units/RoboDatagramPingPongTest.java | 167 ++++++------ .../RoboHttpClientWithResponseTests.java | 78 +++--- .../http/test/units/RoboHttpDynamicTests.java | 40 +-- .../http/test/units/RoboHttpPingPongTest.java | 206 +++++++-------- .../test/units/RoboHttpUnitGetTestApp.java | 243 +++++++++--------- .../units/config/ServiceContainerUnit.java | 17 +- .../SocketMessageDecoratedProducerUnit.java | 201 +++++++-------- .../test/units/config/StringProducerUnit.java | 77 +++--- 14 files changed, 673 insertions(+), 675 deletions(-) diff --git a/robo4j-hw-rpi/pom.xml b/robo4j-hw-rpi/pom.xml index 68cc1fce..ba4be8c1 100644 --- a/robo4j-hw-rpi/pom.xml +++ b/robo4j-hw-rpi/pom.xml @@ -36,14 +36,6 @@ com.robo4j robo4j-math - - org.slf4j - slf4j-api - - - org.slf4j - slf4j-simple - com.pi4j @@ -60,8 +52,17 @@ pi4j-plugin-linuxfs ${pi4j.version} + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine diff --git a/robo4j-socket-http/pom.xml b/robo4j-socket-http/pom.xml index 0d11661d..3dfbf558 100644 --- a/robo4j-socket-http/pom.xml +++ b/robo4j-socket-http/pom.xml @@ -35,8 +35,16 @@ com.robo4j robo4j-core - + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java index 15d791c1..b1c7475a 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java @@ -170,40 +170,20 @@ void nestedObjectToJson() { @Test void nestedJsonToObject() { - TestPerson testPerson2 = new TestPerson(); - testPerson2.setName("name2"); - testPerson2.setValue(5); - - TestPerson testPerson111 = new TestPerson(); - testPerson111.setName("name111"); - testPerson111.setValue(42); - - TestPerson testPerson11 = new TestPerson(); - testPerson11.setName("name11"); - testPerson11.setValue(0); - testPerson11.setChild(testPerson111); - - TestPerson testPerson1 = new TestPerson(); - testPerson1.setName("name1"); - testPerson1.setValue(22); - testPerson1.setChild(testPerson11); - - Map personMap = new LinkedHashMap<>(); - personMap.put("person1", testPerson1); - personMap.put("person2", testPerson2); + Map personMap = getStringTestPersonMap(); long start = System.nanoTime(); NSBWithSimpleCollectionsTypesMessage obj1 = collectionsTypesMessageCodec.decode(testJson); TimeUtils.printTimeDiffNano("decodeFromJson", start); + LOGGER.info("Obj: {}", obj1); + assertEquals(Integer.valueOf(42), obj1.getNumber()); assertEquals("no message", obj1.getMessage()); - assertTrue(!obj1.getActive()); + assertFalse(obj1.getActive()); assertArrayEquals(new String[]{"one", "two"}, obj1.getArray()); assertTrue(obj1.getList().containsAll(Arrays.asList("text1", "text2"))); assertEquals(personMap, obj1.getPersonMap()); - - System.out.println("Obj: " + obj1); } @Test @@ -247,4 +227,29 @@ private static void printJson(String resultJson) { LOGGER.debug("resultJson:{}", resultJson); } + private static Map getStringTestPersonMap() { + TestPerson testPerson2 = new TestPerson(); + testPerson2.setName("name2"); + testPerson2.setValue(5); + + TestPerson testPerson111 = new TestPerson(); + testPerson111.setName("name111"); + testPerson111.setValue(42); + + TestPerson testPerson11 = new TestPerson(); + testPerson11.setName("name11"); + testPerson11.setValue(0); + testPerson11.setChild(testPerson111); + + TestPerson testPerson1 = new TestPerson(); + testPerson1.setName("name1"); + testPerson1.setValue(22); + testPerson1.setChild(testPerson11); + + Map personMap = new LinkedHashMap<>(); + personMap.put("person1", testPerson1); + personMap.put("person2", testPerson2); + return personMap; + } + } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerConfigTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerConfigTests.java index 545f8f44..c520c854 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerConfigTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerConfigTests.java @@ -22,17 +22,13 @@ import com.robo4j.socket.http.util.JsonUtil; import com.robo4j.util.StringConstants; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.Collections; -import java.util.List; -import static org.junit.jupiter.api.Assertions.assertArrayEquals; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertNull; -import static org.junit.jupiter.api.Assertions.assertThrows; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * test for Http Server Unit configuration @@ -41,100 +37,96 @@ * @author Miroslav Wengner (@miragemiko) */ class HttpServerConfigTests { + private static final Logger LOGGER = LoggerFactory.getLogger(HttpServerConfigTests.class); - @Test - void serverConfigurationNullTest() { + @Test + void serverConfigurationNullTest() { + Throwable exception = assertThrows(NullPointerException.class, () -> { + HttpPathMethodDTO serverUnitPathDTO = HttpPathUtils.readServerPathDTO(null); + assertNull(serverUnitPathDTO); + }); - Throwable exception = assertThrows(NullPointerException.class, () -> { - HttpPathMethodDTO serverUnitPathDTO = HttpPathUtils.readServerPathDTO(null); - assertNull(serverUnitPathDTO); - }); + assertNotNull(exception.getMessage()); + } - assertNotNull(exception.getMessage()); - } + @Test + void serverConfigurationEmptyTest() { + Throwable exception = assertThrows(ArrayIndexOutOfBoundsException.class, () -> { + HttpPathUtils.readServerPathDTO(StringConstants.EMPTY); + }); - @Test - void serverConfigurationEmptyTest() { - Throwable exception = assertThrows(ArrayIndexOutOfBoundsException.class, () -> { - HttpPathUtils.readServerPathDTO(StringConstants.EMPTY); - }); + assertEquals("Index 0 out of bounds for length 0", exception.getMessage()); + } - assertEquals("Index 0 out of bounds for length 0", exception.getMessage()); - } + @Test + void serverConfigurationWithoutPropertiesDTOTest() { + var configurationJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}"; + var serverUnitPathDTO = HttpPathUtils.readServerPathDTO(configurationJson); - @Test - void serverConfigurationWithoutPropertiesDTOTest() { + LOGGER.info("serverUnitPathDTO: {}", serverUnitPathDTO); + assertEquals("roboUnit1", serverUnitPathDTO.getRoboUnit()); + assertEquals(HttpMethod.GET, serverUnitPathDTO.getMethod()); + assertTrue(serverUnitPathDTO.getCallbacks().isEmpty()); - String configurationJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}"; - HttpPathMethodDTO serverUnitPathDTO = HttpPathUtils.readServerPathDTO(configurationJson); + } - System.out.println("serverUnitPathDTO: " + serverUnitPathDTO); - assertEquals("roboUnit1", serverUnitPathDTO.getRoboUnit()); - assertEquals(HttpMethod.GET, serverUnitPathDTO.getMethod()); - assertTrue(serverUnitPathDTO.getCallbacks().isEmpty()); + @Test + void serverConfigurationWithPropertiesParsingDTOTest() { + var configurationJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\",\"callbacks\":[\"filter1\",\"filter2\"]}"; + var serverUnitPathDTO = HttpPathUtils.readServerPathDTO(configurationJson); - } + assertEquals("roboUnit1", serverUnitPathDTO.getRoboUnit()); + assertEquals(HttpMethod.GET, serverUnitPathDTO.getMethod()); + assertArrayEquals(Arrays.asList("filter1", "filter2").toArray(), serverUnitPathDTO.getCallbacks().toArray()); + } - @Test - void serverConfigurationWithPropertiesParsingDTOTest() { + @Test + void serverConfigurationNullPathTest() { + var paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, null); - String configurationJson = "{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\",\"callbacks\":[\"filter1\",\"filter2\"]}"; - HttpPathMethodDTO serverUnitPathDTO = HttpPathUtils.readServerPathDTO(configurationJson); + assertNotNull(paths); + assertTrue(paths.isEmpty()); - assertEquals("roboUnit1", serverUnitPathDTO.getRoboUnit()); - assertEquals(HttpMethod.GET, serverUnitPathDTO.getMethod()); - assertArrayEquals(Arrays.asList("filter1", "filter2").toArray(), serverUnitPathDTO.getCallbacks().toArray()); + } - System.out.println("serverUnitPathDTO: " + serverUnitPathDTO); + @Test + void serverConfigurationEmptyPathTest() { + var paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, StringConstants.EMPTY); - } + assertNotNull(paths); + assertTrue(paths.isEmpty()); + } - @Test - void serverConfigurationNullPathTest() { - List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, null); - assertNotNull(paths); - assertTrue(paths.isEmpty()); + @Test + void serverConfigurationWithMultiplePathsWithoutPropertiesTest() { + var configurationJson = "[{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}," + + "{\"roboUnit\":\"roboUnit2\",\"method\":\"POST\"}]"; - } + var expectedPathList = Arrays.asList(new HttpPathMethodDTO("roboUnit1", HttpMethod.GET), + new HttpPathMethodDTO("roboUnit2", HttpMethod.POST)); - @Test - void serverConfigurationEmptyPathTest() { - List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, StringConstants.EMPTY); - assertNotNull(paths); - assertTrue(paths.isEmpty()); + var paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, configurationJson); - } + assertEquals(expectedPathList.size(), paths.size()); + assertArrayEquals(expectedPathList.toArray(), paths.toArray()); + } - @Test - void serverConfigurationWithMultiplePathsWithoutPropertiesTest() { - String configurationJson = "[{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\"}," - + "{\"roboUnit\":\"roboUnit2\",\"method\":\"POST\"}]"; + @Test + void serverConfigurationWithMultiplePathsWithPropertiesTest() { + var configurationJson = "[{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\" , \"callbacks\":[\"filter1\",\"filter2\"]}," + + "{\"roboUnit\":\"roboUnit2\",\"method\":\"POST\"}, {\"roboUnit\":\"roboUnit3\",\"method\":\"GET\",\"callbacks\":[]}]"; - List expectedPathList = Arrays.asList(new HttpPathMethodDTO("roboUnit1", HttpMethod.GET), - new HttpPathMethodDTO("roboUnit2", HttpMethod.POST)); + var expectedPathList = Arrays.asList( + new HttpPathMethodDTO("roboUnit1", HttpMethod.GET, Arrays.asList("filter1", "filter2")), + new HttpPathMethodDTO("roboUnit2", HttpMethod.POST), + new HttpPathMethodDTO("roboUnit3", HttpMethod.GET, Collections.emptyList())); - List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, configurationJson); + var paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, configurationJson); - assertEquals(expectedPathList.size(), paths.size()); - assertArrayEquals(expectedPathList.toArray(), paths.toArray()); - } - - @Test - void serverConfigurationWithMultiplePathsWithPropertiesTest() { - String configurationJson = "[{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\" , \"callbacks\":[\"filter1\",\"filter2\"]}," - + "{\"roboUnit\":\"roboUnit2\",\"method\":\"POST\"}, {\"roboUnit\":\"roboUnit3\",\"method\":\"GET\",\"callbacks\":[]}]"; - - List expectedPathList = Arrays.asList( - new HttpPathMethodDTO("roboUnit1", HttpMethod.GET, Arrays.asList("filter1", "filter2")), - new HttpPathMethodDTO("roboUnit2", HttpMethod.POST), - new HttpPathMethodDTO("roboUnit3", HttpMethod.GET, Collections.emptyList())); - - List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, configurationJson); - System.out.println("paths: " + paths); - - assertNotNull(paths); - assertEquals(expectedPathList.size(), paths.size()); - assertArrayEquals(expectedPathList.toArray(), paths.toArray()); - } + LOGGER.info("paths: {}", paths); + assertNotNull(paths); + assertEquals(expectedPathList.size(), paths.size()); + assertArrayEquals(expectedPathList.toArray(), paths.toArray()); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerUnitTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerUnitTests.java index 7a82e34e..b668170e 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerUnitTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/HttpServerUnitTests.java @@ -19,13 +19,13 @@ import com.robo4j.LifecycleState; import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; -import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; import com.robo4j.socket.http.units.HttpServerUnit; import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static com.robo4j.socket.http.test.units.HttpUnitTests.CODECS_UNITS_TEST_PACKAGE; import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_CODEC_PACKAGES; @@ -38,50 +38,47 @@ * @author Miroslav Wengner (@miragemiko) */ class HttpServerUnitTests { - private static final int PORT = 9000; - private static final String ID_HTTP_SERVER = "empty_server"; + private static final Logger LOGGER = LoggerFactory.getLogger(HttpServerUnitTests.class); + private static final int PORT = 9000; + private static final String ID_HTTP_SERVER = "empty_server"; - @Test - void httpServerUnitNoCodecsPackageTest() throws Exception { + @Test + void httpServerUnitNoCodecsPackageTest() throws Exception { - Throwable exception = assertThrows(RoboBuilderException.class, () -> { - RoboBuilder builder = new RoboBuilder(); + Throwable exception = assertThrows(RoboBuilderException.class, () -> { + var builder = new RoboBuilder(); + var config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT).build(); + builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); + var system = builder.build(); - Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT).build(); - builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); - RoboContext system = builder.build(); + system.start(); + LOGGER.info(SystemUtil.printStateReport(system)); + var systemReference = system.getReference(ID_HTTP_SERVER); + system.shutdown(); - system.start(); - System.out.println("system: State after start:"); - System.out.println(SystemUtil.printStateReport(system)); - RoboReference systemReference = system.getReference(ID_HTTP_SERVER); - system.shutdown(); - System.out.println("system: State after shutdown:"); - System.out.println(SystemUtil.printStateReport(system)); - assertEquals(LifecycleState.SHUTDOWN, systemReference.getState()); - }); + LOGGER.info(SystemUtil.printStateReport(system)); + assertEquals(LifecycleState.SHUTDOWN, systemReference.getState()); + }); - assertEquals("Error initializing RoboUnit", exception.getMessage()); + assertEquals("Error initializing RoboUnit", exception.getMessage()); - } + } - @Test - void httpServerUnitNoPathTest() throws Exception { - RoboBuilder builder = new RoboBuilder(); + @Test + void httpServerUnitNoPathTest() throws Exception { + var builder = new RoboBuilder(); + var config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT) + .addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE).build(); + builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); + var system = builder.build(); - Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT) - .addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE).build(); - builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); - RoboContext system = builder.build(); + system.start(); + LOGGER.info(SystemUtil.printStateReport(system)); + RoboReference systemReference = system.getReference(ID_HTTP_SERVER); + system.shutdown(); - system.start(); - System.out.println("system: State after start:"); - System.out.println(SystemUtil.printStateReport(system)); - RoboReference systemReference = system.getReference(ID_HTTP_SERVER); - system.shutdown(); - System.out.println("system: State after shutdown:"); - System.out.println(SystemUtil.printStateReport(system)); - assertEquals(LifecycleState.SHUTDOWN, systemReference.getState()); - } + LOGGER.info(SystemUtil.printStateReport(system)); + assertEquals(LifecycleState.SHUTDOWN, systemReference.getState()); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java index fefbbe78..2d81c632 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java @@ -22,6 +22,8 @@ import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; @@ -35,6 +37,7 @@ * @author Miroslav Wengner (@miragemiko) */ class RoboDatagramClientTest { + private static final Logger LOGGER = LoggerFactory.getLogger(RoboDatagramClientTest.class); private static final int TIMEOUT = 10; private static final TimeUnit TIME_UNIT = TimeUnit.HOURS; private static final int MAX_NUMBER = 42; @@ -44,17 +47,14 @@ class RoboDatagramClientTest { @Test void datagramClientServerTest() throws Exception { - RoboContext producerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_producer_text.xml"); - RoboContext consumerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_consumer_text.xml"); + var producerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_producer_text.xml"); + var consumerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_consumer_text.xml"); consumerSystem.start(); producerSystem.start(); - System.out.println("consumer: State after start:"); - System.out.println(SystemUtil.printStateReport(consumerSystem)); - - System.out.println("producer: State after start:"); - System.out.println(SystemUtil.printStateReport(producerSystem)); + LOGGER.info(SystemUtil.printStateReport(consumerSystem)); + LOGGER.info(SystemUtil.printStateReport(producerSystem)); RoboReference decoratedProducer = producerSystem.getReference("decoratedProducer"); decoratedProducer.sendMessage(MAX_NUMBER); diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java index 41706041..6c6c5faa 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java @@ -31,16 +31,13 @@ import com.robo4j.socket.http.util.RoboHttpUtils; import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; import static com.robo4j.socket.http.test.units.HttpUnitTests.CODECS_UNITS_TEST_PACKAGE; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_CODEC_PACKAGES; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_HOST; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_SOCKET_PORT; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_UNIT_PATHS_CONFIG; -import static org.junit.jupiter.api.Assertions.assertEquals; +import static com.robo4j.socket.http.util.RoboHttpUtils.*; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -49,84 +46,82 @@ * @author Miroslav Wengner (@miragemiko) */ class RoboDatagramPingPongTest { - - private static final int TIMEOUT = 20; - private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS; - private static final String UDP_CLIENT = "udp_client"; - private static final String UDP_SERVER = "udp_server"; - - private static final int TOTAL_NUMBER = 122; - - @Test - void datagramPingPongTest() throws Exception { - RoboContext pongSystem = configurePongSystem(TOTAL_NUMBER); - RoboContext pingSystem = configurePingSystem(); - - pongSystem.start(); - pingSystem.start(); - System.out.println("UDP pongSystem: State after start:"); - System.out.println(SystemUtil.printStateReport(pongSystem)); - System.out.println("UDP pingSystem: State after start:"); - System.out.println(SystemUtil.printStateReport(pingSystem)); - - RoboReference pongStringConsumerReference = pongSystem.getReference(StringConsumer.NAME); - CountDownLatch totalMessageLatch = pongStringConsumerReference.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); - - RoboReference udpClient = pingSystem.getReference(UDP_CLIENT); - for (int i = 0; i < TOTAL_NUMBER; i++) { - DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), "/units/stringConsumer"); - DatagramDecoratedRequest request = new DatagramDecoratedRequest(denominator); - String message = "{\"message\": \"Hello i:" + i + "\"}"; - request.addMessage(message.getBytes()); - udpClient.sendMessage(request); - } - - totalMessageLatch.await(TIMEOUT, TIME_UNIT); - final int pongConsumerTotalNumber = pongStringConsumerReference.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); - - - System.out.println("UDP pongSystem: State after shutdown:"); - System.out.println(SystemUtil.printStateReport(pongSystem)); - System.out.println("UDP pingSystem: State after shutdown:"); - System.out.println(SystemUtil.printStateReport(pingSystem)); - pingSystem.shutdown(); - pongSystem.shutdown(); - - assertTrue(pongConsumerTotalNumber > 0 && pongConsumerTotalNumber <= TOTAL_NUMBER ); - - } - - private RoboContext configurePingSystem() throws Exception { - RoboBuilder builder = new RoboBuilder(); - - Configuration config = new ConfigurationBuilder().addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE) - .addString(PROPERTY_HOST, "localhost").addInteger(PROPERTY_SOCKET_PORT, RoboHttpUtils.DEFAULT_UDP_PORT) - .addString(PROPERTY_UNIT_PATHS_CONFIG, "[{\"roboUnit\":\"stringConsumer\",\"callbacks\": [\"stringConsumer\"]}]").build(); - builder.add(DatagramClientUnit.class, config, UDP_CLIENT); - - config = ConfigurationFactory.createEmptyConfiguration(); - builder.add(StringConsumer.class, config, StringConsumer.NAME); - - return builder.build(); - } - - /** - * create simple UDP server with consumer unit - * - * - * @return roboContext - * @throws Exception - * exception - */ - private RoboContext configurePongSystem(int totalNumberOfMessage) throws Exception { - RoboBuilder builder = new RoboBuilder(); - Configuration config = new ConfigurationBuilder().addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE) - .addString(PROPERTY_UNIT_PATHS_CONFIG, "[{\"roboUnit\":\"stringConsumer\",\"filters\":[]}]").build(); - builder.add(DatagramServerUnit.class, config, UDP_SERVER); - - config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, totalNumberOfMessage).build(); - builder.add(StringConsumer.class, config, StringConsumer.NAME); - - return builder.build(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboDatagramPingPongTest.class); + private static final int TIMEOUT = 20; + private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS; + private static final String UDP_CLIENT = "udp_client"; + private static final String UDP_SERVER = "udp_server"; + + private static final int TOTAL_NUMBER = 122; + + @Test + void datagramPingPongTest() throws Exception { + var pongSystem = configurePongSystem(TOTAL_NUMBER); + var pingSystem = configurePingSystem(); + + pongSystem.start(); + pingSystem.start(); + LOGGER.info("UDP pongSystem: State after start:"); + LOGGER.info(SystemUtil.printStateReport(pongSystem)); + LOGGER.info("UDP pingSystem: State after start:"); + LOGGER.info(SystemUtil.printStateReport(pingSystem)); + + var pongStringConsumerReference = pongSystem.getReference(StringConsumer.NAME); + var totalMessageLatch = pongStringConsumerReference.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); + + RoboReference udpClient = pingSystem.getReference(UDP_CLIENT); + for (int i = 0; i < TOTAL_NUMBER; i++) { + DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), "/units/stringConsumer"); + DatagramDecoratedRequest request = new DatagramDecoratedRequest(denominator); + String message = "{\"message\": \"Hello i:" + i + "\"}"; + request.addMessage(message.getBytes()); + udpClient.sendMessage(request); + } + + totalMessageLatch.await(TIMEOUT, TIME_UNIT); + final int pongConsumerTotalNumber = pongStringConsumerReference.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); + + + LOGGER.debug("UDP pongSystem: State after shutdown:"); + LOGGER.debug(SystemUtil.printStateReport(pongSystem)); + LOGGER.debug("UDP pingSystem: State after shutdown:"); + LOGGER.debug(SystemUtil.printStateReport(pingSystem)); + pingSystem.shutdown(); + pongSystem.shutdown(); + + assertTrue(pongConsumerTotalNumber > 0 && pongConsumerTotalNumber <= TOTAL_NUMBER); + + } + + private RoboContext configurePingSystem() throws Exception { + RoboBuilder builder = new RoboBuilder(); + + Configuration config = new ConfigurationBuilder().addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE) + .addString(PROPERTY_HOST, "localhost").addInteger(PROPERTY_SOCKET_PORT, RoboHttpUtils.DEFAULT_UDP_PORT) + .addString(PROPERTY_UNIT_PATHS_CONFIG, "[{\"roboUnit\":\"stringConsumer\",\"callbacks\": [\"stringConsumer\"]}]").build(); + builder.add(DatagramClientUnit.class, config, UDP_CLIENT); + + config = ConfigurationFactory.createEmptyConfiguration(); + builder.add(StringConsumer.class, config, StringConsumer.NAME); + + return builder.build(); + } + + /** + * create simple UDP server with consumer unit + * + * @return roboContext + * @throws Exception exception + */ + private RoboContext configurePongSystem(int totalNumberOfMessage) throws Exception { + RoboBuilder builder = new RoboBuilder(); + Configuration config = new ConfigurationBuilder().addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE) + .addString(PROPERTY_UNIT_PATHS_CONFIG, "[{\"roboUnit\":\"stringConsumer\",\"filters\":[]}]").build(); + builder.add(DatagramServerUnit.class, config, UDP_SERVER); + + config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, totalNumberOfMessage).build(); + builder.add(StringConsumer.class, config, StringConsumer.NAME); + + return builder.build(); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpClientWithResponseTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpClientWithResponseTests.java index 3f6738b3..fc113b21 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpClientWithResponseTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpClientWithResponseTests.java @@ -16,17 +16,17 @@ */ package com.robo4j.socket.http.test.units; -import com.robo4j.RoboContext; -import com.robo4j.RoboReference; import com.robo4j.socket.http.test.units.config.SocketMessageDecoratedProducerUnit; import com.robo4j.socket.http.test.units.config.StringConsumer; import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; /** * testing http method GET with response @@ -36,48 +36,52 @@ */ class RoboHttpClientWithResponseTests { - private static final int TIMEOUT = 20; - private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS; - private static final Integer MAX_NUMBER = 20; - // private static final String ROBO_SYSTEM_DESC = - // "[{\"id\":\"stringConsumer\",\"state\":\"STARTED\"},{\"id\":\"httpServer\",\"state\":\"STARTED\"}]"; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpClientWithResponseTests.class); + private static final int TIMEOUT = 20; + private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS; + private static final Integer MAX_NUMBER = 20; + // private static final String ROBO_SYSTEM_DESC = + // "[{\"id\":\"stringConsumer\",\"state\":\"STARTED\"},{\"id\":\"httpServer\",\"state\":\"STARTED\"}]"; - @Test - void simpleRoboSystemGetRequestTest() throws Exception { + @Test + void simpleRoboSystemGetRequestTest() throws Exception { - RoboContext producerSystem = RoboContextUtils - .loadRoboContextByXml("robo_http_client_request_producer_text.xml"); - RoboContext consumerSystem = RoboContextUtils - .loadRoboContextByXml("robo_http_client_request_consumer_text.xml"); + var producerSystem = RoboContextUtils + .loadRoboContextByXml("robo_http_client_request_producer_text.xml"); + var consumerSystem = RoboContextUtils + .loadRoboContextByXml("robo_http_client_request_consumer_text.xml"); - consumerSystem.start(); - producerSystem.start(); + consumerSystem.start(); + producerSystem.start(); - System.out.println("consumer: State after start:"); - System.out.println(SystemUtil.printStateReport(consumerSystem)); + LOGGER.info("consumer: State after start:"); + LOGGER.info(SystemUtil.printStateReport(consumerSystem)); + LOGGER.info("producer: State after start:"); + LOGGER.info(SystemUtil.printStateReport(producerSystem)); - System.out.println("producer: State after start:"); - System.out.println(SystemUtil.printStateReport(producerSystem)); + var decoratedProducer = producerSystem.getReference("decoratedProducer"); + var producerSetupLatch = decoratedProducer + .getAttribute(SocketMessageDecoratedProducerUnit.DESCRIPTOR_SETUP_LATCH).get(); + decoratedProducer.sendMessage(MAX_NUMBER); + var producerConfigured = producerSetupLatch.await(TIMEOUT, TIME_UNIT); + var producerLatch = decoratedProducer + .getAttribute(SocketMessageDecoratedProducerUnit.DESCRIPTOR_MESSAGES_LATCH).get(); + var messagesSent = producerLatch.await(TIMEOUT, TIME_UNIT); - RoboReference decoratedProducer = producerSystem.getReference("decoratedProducer"); - CountDownLatch producerSetupLatch = decoratedProducer - .getAttribute(SocketMessageDecoratedProducerUnit.DESCRIPTOR_SETUP_LATCH).get(); - decoratedProducer.sendMessage(MAX_NUMBER); - producerSetupLatch.await(TIMEOUT, TIME_UNIT); - CountDownLatch producerLatch = decoratedProducer - .getAttribute(SocketMessageDecoratedProducerUnit.DESCRIPTOR_MESSAGES_LATCH).get(); - producerLatch.await(TIMEOUT, TIME_UNIT); + var producerStringConsumer = producerSystem.getReference(StringConsumer.NAME); + var messagesLatchStringConsumer = producerStringConsumer + .getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); + var messagesReceived = messagesLatchStringConsumer.await(TIMEOUT, TIME_UNIT); - final RoboReference producerStringConsumer = producerSystem.getReference(StringConsumer.NAME); - final CountDownLatch messagesLatchStringConsumer = producerStringConsumer - .getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); - messagesLatchStringConsumer.await(TIMEOUT, TIME_UNIT); + var totalNumber = producerStringConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); - final Integer totalNumber = producerStringConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); - assertEquals(MAX_NUMBER, totalNumber); + assertTrue(producerConfigured); + assertTrue(messagesSent); + assertTrue(messagesReceived); + assertEquals(MAX_NUMBER, totalNumber); - producerSystem.shutdown(); - consumerSystem.shutdown(); - } + producerSystem.shutdown(); + consumerSystem.shutdown(); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java index e0c909be..b720401d 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java @@ -33,6 +33,8 @@ import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.CountDownLatch; import java.util.concurrent.ExecutionException; @@ -49,7 +51,6 @@ * @author Miro Wengner (@miragemiko) */ class RoboHttpDynamicTests { - private static final int TIMEOUT = 20; private static final TimeUnit TIME_UNIT = TimeUnit.HOURS; private static final String ID_HTTP_SERVER = "http"; @@ -59,6 +60,7 @@ class RoboHttpDynamicTests { private static final int MESSAGES_NUMBER = 42; private static final String HOST_SYSTEM = "localhost"; static final String JSON_STRING = "{\"value\":\"stop\"}"; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpDynamicTests.class); private static final String DECORATED_PRODUCER = "decoratedProducer"; /** @@ -73,34 +75,32 @@ class RoboHttpDynamicTests { void simpleHttpNonUnitTest() throws Exception { /* tested system configuration */ - RoboContext mainSystem = getServerRoboSystem(MESSAGES_NUMBER); + var mainSystem = getServerRoboSystem(MESSAGES_NUMBER); /* system which is testing main system */ - RoboContext clientSystem = getClientRoboSystem(); - - System.out.println("Client system state after start:"); - System.out.println(SystemUtil.printStateReport(clientSystem)); + var clientSystem = getClientRoboSystem(); - System.out.println("Main system state after start:"); - System.out.println(SystemUtil.printStateReport(mainSystem)); + LOGGER.info("Client system state after start:"); + LOGGER.info(SystemUtil.printStateReport(clientSystem)); + LOGGER.info("Main system state after start:"); + LOGGER.info(SystemUtil.printStateReport(mainSystem)); /* client system sending a messages to the main system */ - RoboReference decoratedProducer = clientSystem.getReference(DECORATED_PRODUCER); + var decoratedProducer = clientSystem.getReference(DECORATED_PRODUCER); decoratedProducer.sendMessage(MESSAGES_NUMBER); // TODO: review how to receiving attributes - CountDownLatch countDownLatchDecoratedProducer = getAttributeOrTimeout(decoratedProducer, SocketMessageDecoratedProducerUnit.DESCRIPTOR_MESSAGES_LATCH); + var countDownLatchDecoratedProducer = getAttributeOrTimeout(decoratedProducer, SocketMessageDecoratedProducerUnit.DESCRIPTOR_MESSAGES_LATCH); var messagesProduced = countDownLatchDecoratedProducer.await(TIMEOUT, TIME_UNIT); - - final RoboReference stringConsumer = mainSystem.getReference(StringConsumer.NAME); - final CountDownLatch countDownLatch = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_LATCH); + var stringConsumer = mainSystem.getReference(StringConsumer.NAME); + var countDownLatch = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_LATCH); var messagesReceived = countDownLatch.await(TIMEOUT, TIME_UNIT); - final int receivedMessages = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_TOTAL); + var receivedMessages = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_TOTAL); clientSystem.shutdown(); mainSystem.shutdown(); - System.out.println("System is Down!"); + LOGGER.info("System is Down!"); assertTrue(messagesProduced); assertTrue(messagesReceived); assertNotNull(mainSystem.getUnits()); @@ -121,8 +121,8 @@ void pingExternalSystem() throws Exception { RoboContext pingSystemContext = pingSystemBuilder.build(); pingSystemContext.start(); - System.out.println("PingSystem state after start:"); - System.out.println(SystemUtil.printStateReport(pingSystemContext)); + LOGGER.info("PingSystem state after start:"); + LOGGER.info(SystemUtil.printStateReport(pingSystemContext)); RoboReference httpClient = pingSystemContext.getReference(ID_CLIENT_UNIT); @@ -136,8 +136,8 @@ void pingExternalSystem() throws Exception { } Thread.sleep(1000); pingSystemContext.stop(); - System.out.println("PingSystem state after stop:"); - System.out.println(SystemUtil.printStateReport(pingSystemContext)); + LOGGER.info("PingSystem state after stop:"); + LOGGER.info(SystemUtil.printStateReport(pingSystemContext)); } @@ -166,7 +166,7 @@ private RoboContext getServerRoboSystem(int totalMessageNumber) throws Exception assertEquals(LifecycleState.INITIALIZED, result.getState()); result.start(); - System.out.println(SystemUtil.printSocketEndPoint(result.getReference(ID_HTTP_SERVER), + LOGGER.info(SystemUtil.printSocketEndPoint(result.getReference(ID_HTTP_SERVER), result.getReference(ID_TARGET_UNIT))); return result; } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpPingPongTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpPingPongTest.java index 5a5e8a8d..483e3100 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpPingPongTest.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpPingPongTest.java @@ -18,7 +18,6 @@ import com.robo4j.RoboBuilder; import com.robo4j.RoboContext; -import com.robo4j.RoboReference; import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; import com.robo4j.socket.http.HttpMethod; @@ -30,15 +29,14 @@ import com.robo4j.socket.http.util.HttpPathConfigJsonBuilder; import com.robo4j.util.SystemUtil; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_HOST; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_SOCKET_PORT; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_TARGET; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_UNIT_PATHS_CONFIG; +import static com.robo4j.socket.http.util.RoboHttpUtils.*; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; /** * Ping Pong test from outside/foreign unit is send signal. The signal has been @@ -52,104 +50,100 @@ * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ -public class RoboHttpPingPongTest { - - public static final String HOST_SYSTEM = "127.0.0.1"; - private static final int TIMEOUT = 10; - private static final TimeUnit TIME_UNIT = TimeUnit.HOURS; - private static final String ID_HTTP_CLIENT = "http_client"; - private static final String ID_HTTP_SERVER = "http_server"; - private static final String CONTROLLER_PING_PONG = "controller"; - private static final int PORT = 8042; - private static final int MESSAGES = 50; - private static final String REQUEST_CONSUMER = "request_consumer"; - private static final String DECORATED_PRODUCER = "decoratedProducer"; - - public void runPongServer() throws Exception { - RoboContext system = configurePongSystem(0); - system.start(); - System.out.println("systemPong: State after start:"); - System.out.println(SystemUtil.printStateReport(system)); - System.out.println("Press ..."); - System.in.read(); - system.shutdown(); - - } - - @Test - void pingPongTest() throws Exception { - - RoboContext systemPong = configurePongSystem(MESSAGES); - RoboContext systemPing = configurePingSystem(); - - systemPong.start(); - System.out.println("systemPong: State after start:"); - System.out.println(SystemUtil.printStateReport(systemPong)); - - systemPing.start(); - System.out.println("systemPing: State after start:"); - System.out.println(SystemUtil.printStateReport(systemPing)); - - System.out.println("systemPing: send messages"); - RoboReference decoratedProducer = systemPing.getReference(DECORATED_PRODUCER); - decoratedProducer.sendMessage(MESSAGES); - - RoboReference pongConsumer = systemPong.getReference(REQUEST_CONSUMER); - CountDownLatch attributeFuture = pongConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); - - attributeFuture.await(TIMEOUT, TIME_UNIT); - System.out.println("systemPing : Going Down!"); - systemPing.stop(); - systemPing.shutdown(); - - System.out.println("systemPong : Going Down!"); - - final int number = pongConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); - systemPong.stop(); - assertEquals(MESSAGES, number); - System.out.println("PingPong is down!"); - systemPong.shutdown(); - - } - - // Private Methods - private RoboContext configurePongSystem(int totalMessageNumber) throws Exception { - RoboBuilder builder = new RoboBuilder(); - - final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath(CONTROLLER_PING_PONG, - HttpMethod.POST); - - Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT) - .addString("packages", HttpUnitTests.CODECS_UNITS_TEST_PACKAGE).addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) - .build(); - - builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); - - config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, totalMessageNumber) - .build(); - builder.add(StringConsumer.class, config, REQUEST_CONSUMER); - - config = new ConfigurationBuilder().addString("target", REQUEST_CONSUMER).build(); - builder.add(HttpCommandTestController.class, config, CONTROLLER_PING_PONG); - - return builder.build(); - } - - private RoboContext configurePingSystem() throws Exception { - - /* system which is testing main system */ - RoboBuilder builder = new RoboBuilder(); - - Configuration config = new ConfigurationBuilder().addString(PROPERTY_HOST, HOST_SYSTEM) - .addInteger(PROPERTY_SOCKET_PORT, PORT).build(); - builder.add(HttpClientUnit.class, config, ID_HTTP_CLIENT); - - config = new ConfigurationBuilder().addString(PROPERTY_TARGET, ID_HTTP_CLIENT) - .addString(PROPERTY_UNIT_PATHS_CONFIG, - "[{\"roboUnit\":\"" + CONTROLLER_PING_PONG + "\",\"method\":\"POST\"}]") - .addString("message", RoboHttpDynamicTests.JSON_STRING).build(); - builder.add(SocketMessageDecoratedProducerUnit.class, config, DECORATED_PRODUCER); - - return builder.build(); - } +class RoboHttpPingPongTest { + public static final String HOST_SYSTEM = "127.0.0.1"; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpPingPongTest.class); + private static final int TIMEOUT = 10; + private static final TimeUnit TIME_UNIT = TimeUnit.HOURS; + private static final String ID_HTTP_CLIENT = "http_client"; + private static final String ID_HTTP_SERVER = "http_server"; + private static final String CONTROLLER_PING_PONG = "controller"; + private static final int PORT = 8042; + private static final int MESSAGES = 50; + private static final String REQUEST_CONSUMER = "request_consumer"; + private static final String DECORATED_PRODUCER = "decoratedProducer"; + + public void runPongServer() throws Exception { + var system = configurePongSystem(0); + system.start(); + + LOGGER.info("systemPong: State after start:"); + LOGGER.info(SystemUtil.printStateReport(system)); + LOGGER.info("Press ..."); + System.in.read(); + system.shutdown(); + } + + @Test + void pingPongTest() throws Exception { + + var systemPong = configurePongSystem(MESSAGES); + var systemPing = configurePingSystem(); + + systemPong.start(); + LOGGER.info(SystemUtil.printStateReport(systemPong)); + systemPing.start(); + LOGGER.info(SystemUtil.printStateReport(systemPing)); + LOGGER.info("systemPing: send messages"); + var decoratedProducer = systemPing.getReference(DECORATED_PRODUCER); + decoratedProducer.sendMessage(MESSAGES); + + var pongConsumer = systemPong.getReference(REQUEST_CONSUMER); + var attributeFuture = pongConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(); + var receivedMessages = attributeFuture.await(TIMEOUT, TIME_UNIT); + LOGGER.info("systemPing : Going Down!"); + systemPing.stop(); + systemPing.shutdown(); + + LOGGER.info("systemPong : Going Down!"); + final int number = pongConsumer.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(); + systemPong.stop(); + + assertTrue(receivedMessages); + assertEquals(MESSAGES, number); + LOGGER.info("PingPong is down!"); + systemPong.shutdown(); + + } + + // Private Methods + private RoboContext configurePongSystem(int totalMessageNumber) throws Exception { + RoboBuilder builder = new RoboBuilder(); + + final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath(CONTROLLER_PING_PONG, + HttpMethod.POST); + + Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, PORT) + .addString("packages", HttpUnitTests.CODECS_UNITS_TEST_PACKAGE).addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) + .build(); + + builder.add(HttpServerUnit.class, config, ID_HTTP_SERVER); + + config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, totalMessageNumber) + .build(); + builder.add(StringConsumer.class, config, REQUEST_CONSUMER); + + config = new ConfigurationBuilder().addString("target", REQUEST_CONSUMER).build(); + builder.add(HttpCommandTestController.class, config, CONTROLLER_PING_PONG); + + return builder.build(); + } + + private RoboContext configurePingSystem() throws Exception { + + /* system which is testing main system */ + RoboBuilder builder = new RoboBuilder(); + + Configuration config = new ConfigurationBuilder().addString(PROPERTY_HOST, HOST_SYSTEM) + .addInteger(PROPERTY_SOCKET_PORT, PORT).build(); + builder.add(HttpClientUnit.class, config, ID_HTTP_CLIENT); + + config = new ConfigurationBuilder().addString(PROPERTY_TARGET, ID_HTTP_CLIENT) + .addString(PROPERTY_UNIT_PATHS_CONFIG, + "[{\"roboUnit\":\"" + CONTROLLER_PING_PONG + "\",\"method\":\"POST\"}]") + .addString("message", RoboHttpDynamicTests.JSON_STRING).build(); + builder.add(SocketMessageDecoratedProducerUnit.class, config, DECORATED_PRODUCER); + + return builder.build(); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpUnitGetTestApp.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpUnitGetTestApp.java index 2f6db958..ad2302c7 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpUnitGetTestApp.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpUnitGetTestApp.java @@ -32,12 +32,12 @@ import com.robo4j.socket.http.units.HttpServerUnit; import com.robo4j.socket.http.util.HttpPathConfigJsonBuilder; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static com.robo4j.socket.http.test.units.HttpUnitTests.CODECS_UNITS_TEST_PACKAGE; import static com.robo4j.socket.http.test.units.RoboHttpPingPongTest.HOST_SYSTEM; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_HOST; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_SOCKET_PORT; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_UNIT_PATHS_CONFIG; +import static com.robo4j.socket.http.util.RoboHttpUtils.*; /** * RoboHttpUnitGetTestApp should test Http get requests @@ -45,23 +45,22 @@ * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ -public class RoboHttpUnitGetTestApp { - - private static final int SERVER_PORT = 8061; - private static final String UNIT_ID_HTTP_CLIENT = "http_client"; - - /** - * Run the system with only server unit - * - * @throws Exception - * exception - */ - public void systemWithHttpServerOnlyTest() throws Exception { - final String httpServerUnitName = "http_server"; - final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath(httpServerUnitName, - HttpMethod.GET); - - //@formatter:off +class RoboHttpUnitGetTestApp { + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpUnitGetTestApp.class); + private static final int SERVER_PORT = 8061; + private static final String UNIT_ID_HTTP_CLIENT = "http_client"; + + /** + * Run the system with only server unit + * + * @throws Exception exception + */ + public void systemWithHttpServerOnlyTest() throws Exception { + final String httpServerUnitName = "http_server"; + final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath(httpServerUnitName, + HttpMethod.GET); + + //@formatter:off Configuration systemConfiguration = new ConfigurationBuilder() .addInteger("poolSizeScheduler", 3) .addInteger("poolSizeWorker", 2) @@ -70,35 +69,32 @@ public void systemWithHttpServerOnlyTest() throws Exception { RoboBuilder builder = new RoboBuilder("roboSystem1", systemConfiguration); //@formatter:on - //@formatter:off + //@formatter:off Configuration config = new ConfigurationBuilder() .addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT) .addString("packages", "com.robo4j.socket.http.codec") .addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) .build(); //@formatter:on - builder.add(HttpServerUnit.class, config, httpServerUnitName); - RoboContext system = builder.build(); - - system.start(); - System.out.println("systemPong: State after start:"); - System.out.println(SystemUtil.printStateReport(system)); - System.out.println("Press ..."); - System.in.read(); - system.shutdown(); - } - - /** - * Run the system with known attributes - * - * @throws Exception - * exception - */ - public void oneKnownAttributeTest() throws Exception { - final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath("controller", - HttpMethod.GET); - - //@formatter:off + builder.add(HttpServerUnit.class, config, httpServerUnitName); + RoboContext system = builder.build(); + + system.start(); + printSystemReport(system); + System.in.read(); + system.shutdown(); + } + + /** + * Run the system with known attributes + * + * @throws Exception exception + */ + public void oneKnownAttributeTest() throws Exception { + final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath("controller", + HttpMethod.GET); + + //@formatter:off Configuration systemConfiguration = new ConfigurationBuilder() .addInteger("poolSizeScheduler", 4) .addInteger("poolSizeWorker", 2) @@ -107,89 +103,92 @@ public void oneKnownAttributeTest() throws Exception { RoboBuilder builder = new RoboBuilder("roboSystem1", systemConfiguration); //@formatter:on - //@formatter:off + //@formatter:off Configuration config = new ConfigurationBuilder() .addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT) .addString("packages", CODECS_UNITS_TEST_PACKAGE) .addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) .build(); //@formatter:on - builder.add(HttpServerUnit.class, config, "http_server"); - - config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, 1).build(); - builder.add(StringConsumer.class, config, "request_consumer"); - - config = new ConfigurationBuilder().addString("target", "request_consumer").build(); - builder.add(HttpOneAttributeGetController.class, config, "controller"); - - RoboContext system = builder.build(); - - system.start(); - System.out.println("systemPong: State after start:"); - System.out.println(SystemUtil.printStateReport(system)); - System.out.println("Press ..."); - System.in.read(); - system.shutdown(); - - } - - /** - * Run the system with known attributes - * - * @throws Exception - * exception - */ - public void twoKnownAttributesTest() throws Exception { - RoboContext systemGetProvider = twoAttributesSystem(); - RoboContext systemGetAccessor = attributeRequestSystem(); - - systemGetProvider.start(); - System.out.println("systemGetProvider: State after start:"); - System.out.println(SystemUtil.printStateReport(systemGetProvider)); - systemGetAccessor.start(); - System.out.println("systemGetAccessor: State after start:"); - System.out.println(SystemUtil.printStateReport(systemGetAccessor)); - - RoboReference httpClient = systemGetAccessor.getReference(UNIT_ID_HTTP_CLIENT); - - HttpRequestDenominator denominator = new HttpRequestDenominator(HttpMethod.GET, - "/units/controller?attributes=number", HttpVersion.HTTP_1_1); - HttpDecoratedRequest request = new HttpDecoratedRequest(denominator); - request.addCallback(StringConsumer.NAME); - httpClient.sendMessage(request); - - System.out.println("Press ..."); - System.in.read(); - systemGetProvider.shutdown(); - } - - private RoboContext attributeRequestSystem() throws Exception { - RoboBuilder builder = new RoboBuilder(); - - Configuration config = new ConfigurationBuilder().addString(PROPERTY_HOST, HOST_SYSTEM) - .addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT).build(); - builder.add(HttpClientUnit.class, config, UNIT_ID_HTTP_CLIENT); - builder.add(StringConsumer.class, StringConsumer.NAME); - return builder.build(); - } - - private RoboContext twoAttributesSystem() throws Exception { - Configuration systemConfiguration = new ConfigurationBuilder().addInteger("poolSizeScheduler", 4) - .addInteger("poolSizeWorker", 2).addInteger("poolSizeBlocking", 3).build(); - RoboBuilder builder = new RoboBuilder(systemConfiguration); - - final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath("controller", - HttpMethod.GET); - Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT) - .addString("packages", CODECS_UNITS_TEST_PACKAGE).addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) - .build(); - builder.add(HttpServerUnit.class, config, "http_server"); - - config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, 1).build(); - builder.add(StringConsumer.class, config, "request_consumer"); - - config = new ConfigurationBuilder().addString("target", "request_consumer").build(); - builder.add(HttpTwoAttributesGetController.class, config, "controller"); - return builder.build(); - } + builder.add(HttpServerUnit.class, config, "http_server"); + + config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, 1).build(); + builder.add(StringConsumer.class, config, "request_consumer"); + + config = new ConfigurationBuilder().addString("target", "request_consumer").build(); + builder.add(HttpOneAttributeGetController.class, config, "controller"); + + RoboContext system = builder.build(); + + system.start(); + printSystemReport(system); + System.in.read(); + system.shutdown(); + + } + + /** + * Run the system with known attributes + * + * @throws Exception exception + */ + public void twoKnownAttributesTest() throws Exception { + RoboContext systemGetProvider = twoAttributesSystem(); + RoboContext systemGetAccessor = attributeRequestSystem(); + + systemGetProvider.start(); + LOGGER.info("systemGetProvider: State after start:"); + LOGGER.info(SystemUtil.printStateReport(systemGetProvider)); + systemGetAccessor.start(); + LOGGER.info("systemGetAccessor: State after start:"); + LOGGER.info(SystemUtil.printStateReport(systemGetAccessor)); + + RoboReference httpClient = systemGetAccessor.getReference(UNIT_ID_HTTP_CLIENT); + + HttpRequestDenominator denominator = new HttpRequestDenominator(HttpMethod.GET, + "/units/controller?attributes=number", HttpVersion.HTTP_1_1); + HttpDecoratedRequest request = new HttpDecoratedRequest(denominator); + request.addCallback(StringConsumer.NAME); + httpClient.sendMessage(request); + + printSystemReport(systemGetProvider); + System.in.read(); + systemGetProvider.shutdown(); + } + + private RoboContext attributeRequestSystem() throws Exception { + RoboBuilder builder = new RoboBuilder(); + + Configuration config = new ConfigurationBuilder().addString(PROPERTY_HOST, HOST_SYSTEM) + .addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT).build(); + builder.add(HttpClientUnit.class, config, UNIT_ID_HTTP_CLIENT); + builder.add(StringConsumer.class, StringConsumer.NAME); + return builder.build(); + } + + private RoboContext twoAttributesSystem() throws Exception { + Configuration systemConfiguration = new ConfigurationBuilder().addInteger("poolSizeScheduler", 4) + .addInteger("poolSizeWorker", 2).addInteger("poolSizeBlocking", 3).build(); + RoboBuilder builder = new RoboBuilder(systemConfiguration); + + final HttpPathConfigJsonBuilder pathBuilder = HttpPathConfigJsonBuilder.Builder().addPath("controller", + HttpMethod.GET); + Configuration config = new ConfigurationBuilder().addInteger(PROPERTY_SOCKET_PORT, SERVER_PORT) + .addString("packages", CODECS_UNITS_TEST_PACKAGE).addString(PROPERTY_UNIT_PATHS_CONFIG, pathBuilder.build()) + .build(); + builder.add(HttpServerUnit.class, config, "http_server"); + + config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, 1).build(); + builder.add(StringConsumer.class, config, "request_consumer"); + + config = new ConfigurationBuilder().addString("target", "request_consumer").build(); + builder.add(HttpTwoAttributesGetController.class, config, "controller"); + return builder.build(); + } + + private static void printSystemReport(RoboContext system) { + LOGGER.info("systemPong: State after start:"); + LOGGER.info(SystemUtil.printStateReport(system)); + LOGGER.info("Press ..."); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/ServiceContainerUnit.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/ServiceContainerUnit.java index 348e6fc6..021cd97d 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/ServiceContainerUnit.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/ServiceContainerUnit.java @@ -17,8 +17,10 @@ package com.robo4j.socket.http.test.units.config; import com.robo4j.RoboContext; -import com.robo4j.socket.http.units.ExtendedRoboUnit; import com.robo4j.socket.http.test.units.config.service.NumberService; +import com.robo4j.socket.http.units.ExtendedRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Objects; @@ -28,14 +30,15 @@ * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ -public class ServiceContainerUnit extends ExtendedRoboUnit { - +// TODO : review usage +class ServiceContainerUnit extends ExtendedRoboUnit { public static final String NAME = "containerUnit"; public static final String NUMBER_SERVICE = "numberService"; + private static final Logger LOGGER = LoggerFactory.getLogger(ServiceContainerUnit.class); public ServiceContainerUnit(RoboContext context, String id) { - super(Object.class, context, id); - } + super(Object.class, context, id); + } @Override public void start() { @@ -46,8 +49,8 @@ public void start() { @Override public void onMessage(Object message) { - System.out.println(getClass().getSimpleName() + ":message:" + message); - System.out.println(getClass().getSimpleName() + ":number:" + getService().getNumber()); + LOGGER.info("message:{}", message); + LOGGER.info("number:{}", getService().getNumber()); } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java index fc3551e3..c6f10176 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java @@ -16,11 +16,7 @@ */ package com.robo4j.socket.http.test.units.config; -import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; -import com.robo4j.DefaultAttributeDescriptor; -import com.robo4j.RoboContext; -import com.robo4j.RoboUnit; +import com.robo4j.*; import com.robo4j.configuration.Configuration; import com.robo4j.socket.http.HttpVersion; import com.robo4j.socket.http.dto.HttpPathMethodDTO; @@ -28,12 +24,14 @@ import com.robo4j.socket.http.message.DatagramDenominator; import com.robo4j.socket.http.message.HttpDecoratedRequest; import com.robo4j.socket.http.message.HttpRequestDenominator; +import com.robo4j.socket.http.test.units.config.enums.CommunicationType; import com.robo4j.socket.http.units.ClientContext; import com.robo4j.socket.http.units.ClientPathConfig; -import com.robo4j.socket.http.test.units.config.enums.CommunicationType; import com.robo4j.socket.http.util.DatagramBodyType; import com.robo4j.socket.http.util.HttpPathUtils; import com.robo4j.socket.http.util.JsonUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.List; import java.util.concurrent.CountDownLatch; @@ -50,110 +48,109 @@ * @author Miro Wengner (@miragemiko) */ public class SocketMessageDecoratedProducerUnit extends RoboUnit { + public static final String PROP_COMMUNICATION_TYPE = "communicationType"; + public static final String ATTR_TOTAL_SENT_MESSAGES = "getNumberOfSentMessages"; + public static final String ATTR_MESSAGES_LATCH = "messagesLatch"; + public static final String ATTR_SETUP_LATCH = "setupLatch"; + public static final DefaultAttributeDescriptor DESCRIPTOR_MESSAGES_LATCH = DefaultAttributeDescriptor + .create(CountDownLatch.class, ATTR_MESSAGES_LATCH); + public static final DefaultAttributeDescriptor DESCRIPTOR_SETUP_LATCH = DefaultAttributeDescriptor + .create(CountDownLatch.class, ATTR_SETUP_LATCH); - public static final String PROP_COMMUNICATION_TYPE = "communicationType"; - public static final String ATTR_TOTAL_SENT_MESSAGES = "getNumberOfSentMessages"; - public static final String ATTR_MESSAGES_LATCH = "messagesLatch"; - public static final String ATTR_SETUP_LATCH = "setupLatch"; - public static final DefaultAttributeDescriptor DESCRIPTOR_MESSAGES_LATCH = DefaultAttributeDescriptor - .create(CountDownLatch.class, ATTR_MESSAGES_LATCH); - public static final DefaultAttributeDescriptor DESCRIPTOR_SETUP_LATCH = DefaultAttributeDescriptor - .create(CountDownLatch.class, ATTR_SETUP_LATCH); - - private static final int DEFAULT = 0; - private final ClientContext clientContext = new ClientContext(); - private volatile CountDownLatch setupLatch = new CountDownLatch(1); - private volatile CountDownLatch messagesLatch; - private AtomicInteger counter; - private String target; - private String message; - private CommunicationType type; + private static final Logger LOGGER = LoggerFactory.getLogger(SocketMessageDecoratedProducerUnit.class); + private static final int DEFAULT = 0; + private final ClientContext clientContext = new ClientContext(); + private volatile CountDownLatch setupLatch = new CountDownLatch(1); + private volatile CountDownLatch messagesLatch; + private AtomicInteger counter; + private String target; + private String message; + private CommunicationType type; - public SocketMessageDecoratedProducerUnit(RoboContext context, String id) { - super(Integer.class, context, id); - } + public SocketMessageDecoratedProducerUnit(RoboContext context, String id) { + super(Integer.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString(PROPERTY_TARGET, null); - message = configuration.getString("message", null); + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString(PROPERTY_TARGET, null); + message = configuration.getString("message", null); - List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, - configuration.getString(PROPERTY_UNIT_PATHS_CONFIG, null)); - HttpPathUtils.updateHttpClientContextPaths(clientContext, paths); - counter = new AtomicInteger(DEFAULT); - type = CommunicationType.valueOf(configuration.getString(PROP_COMMUNICATION_TYPE, CommunicationType.HTTP.toString()).toUpperCase()); - } + List paths = JsonUtil.readPathConfig(HttpPathMethodDTO.class, + configuration.getString(PROPERTY_UNIT_PATHS_CONFIG, null)); + HttpPathUtils.updateHttpClientContextPaths(clientContext, paths); + counter = new AtomicInteger(DEFAULT); + type = CommunicationType.valueOf(configuration.getString(PROP_COMMUNICATION_TYPE, CommunicationType.HTTP.toString()).toUpperCase()); + } - /** - * produces desired number of GET request on RoboSystem - * - * @param number - * number of get messages - */ - @Override - public void onMessage(Integer number) { - messagesLatch = new CountDownLatch(number); - setupLatch.countDown(); - clientContext.getPathConfigs().forEach(pathConfig -> { - IntStream.range(DEFAULT, number).forEach(i -> { - switch (type){ - case HTTP: - getContext().getReference(target).sendMessage(getHttpRequest(pathConfig)); - break; - case DATAGRAM: - getContext().getReference(target).sendMessage(getDatagramRequest(pathConfig)); - break; - default: - throw new IllegalStateException("not allowed"); - } - messagesLatch.countDown(); - }); - System.out.println(getClass().getSimpleName() + "messages: " + number); - }); + /** + * produces desired number of GET request on RoboSystem + * + * @param number number of get messages + */ + @Override + public void onMessage(Integer number) { + messagesLatch = new CountDownLatch(number); + setupLatch.countDown(); + clientContext.getPathConfigs().forEach(pathConfig -> { + IntStream.range(DEFAULT, number).forEach(i -> { + switch (type) { + case HTTP: + getContext().getReference(target).sendMessage(getHttpRequest(pathConfig)); + break; + case DATAGRAM: + getContext().getReference(target).sendMessage(getDatagramRequest(pathConfig)); + break; + default: + throw new IllegalStateException("not allowed"); + } + messagesLatch.countDown(); + }); + LOGGER.info("messages: {}", number); + }); - } + } - private DatagramDecoratedRequest getDatagramRequest(ClientPathConfig pathConfig){ - final DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), pathConfig.getPath()); - final DatagramDecoratedRequest result = new DatagramDecoratedRequest(denominator); - result.addMessage(message.getBytes()); - return result; - } + private DatagramDecoratedRequest getDatagramRequest(ClientPathConfig pathConfig) { + final DatagramDenominator denominator = new DatagramDenominator(DatagramBodyType.JSON.getType(), pathConfig.getPath()); + final DatagramDecoratedRequest result = new DatagramDecoratedRequest(denominator); + result.addMessage(message.getBytes()); + return result; + } - private HttpDecoratedRequest getHttpRequest(ClientPathConfig pathConfig){ - HttpRequestDenominator denominator = new HttpRequestDenominator(pathConfig.getMethod(), - pathConfig.getPath(), HttpVersion.HTTP_1_1); - HttpDecoratedRequest result = new HttpDecoratedRequest(denominator); - switch (pathConfig.getMethod()) { - case GET: - result.addCallbacks(pathConfig.getCallbacks()); - break; - case POST: - result.addMessage(message); - break; - default: - throw new IllegalStateException("not allowed state: " + pathConfig); - } - return result; - } + private HttpDecoratedRequest getHttpRequest(ClientPathConfig pathConfig) { + HttpRequestDenominator denominator = new HttpRequestDenominator(pathConfig.getMethod(), + pathConfig.getPath(), HttpVersion.HTTP_1_1); + HttpDecoratedRequest result = new HttpDecoratedRequest(denominator); + switch (pathConfig.getMethod()) { + case GET: + result.addCallbacks(pathConfig.getCallbacks()); + break; + case POST: + result.addMessage(message); + break; + default: + throw new IllegalStateException("not allowed state: " + pathConfig); + } + return result; + } - @SuppressWarnings("unchecked") - @Override - public synchronized R onGetAttribute(AttributeDescriptor attribute) { - if (attribute.getAttributeName().equals(ATTR_TOTAL_SENT_MESSAGES) - && attribute.getAttributeType() == Integer.class) { - return (R) (Integer) counter.get(); - } - if (attribute.getAttributeName().equals(ATTR_MESSAGES_LATCH) - && attribute.getAttributeType() == CountDownLatch.class) { - return (R) messagesLatch; - } - if (attribute.getAttributeName().equals(ATTR_SETUP_LATCH) - && attribute.getAttributeType() == CountDownLatch.class) { - return (R) setupLatch; - } - return null; - } + @SuppressWarnings("unchecked") + @Override + public synchronized R onGetAttribute(AttributeDescriptor attribute) { + if (attribute.getAttributeName().equals(ATTR_TOTAL_SENT_MESSAGES) + && attribute.getAttributeType() == Integer.class) { + return (R) (Integer) counter.get(); + } + if (attribute.getAttributeName().equals(ATTR_MESSAGES_LATCH) + && attribute.getAttributeType() == CountDownLatch.class) { + return (R) messagesLatch; + } + if (attribute.getAttributeName().equals(ATTR_SETUP_LATCH) + && attribute.getAttributeType() == CountDownLatch.class) { + return (R) setupLatch; + } + return null; + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java index b34f4419..7f9e67f4 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java @@ -21,6 +21,8 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.atomic.AtomicInteger; @@ -29,47 +31,48 @@ * @author Miroslav Wengner (@miragemiko) */ public class StringProducerUnit extends RoboUnit { - /* default sent messages */ - private static final int DEFAULT = 0; - private AtomicInteger counter; - private String target; + private static final Logger LOGGER = LoggerFactory.getLogger(StringProducerUnit.class); + /* default sent messages */ + private static final int DEFAULT = 0; + private AtomicInteger counter; + private String target; - /** - * @param context - * @param id - */ - public StringProducerUnit(RoboContext context, String id) { - super(String.class, context, id); - } + /** + * @param context + * @param id + */ + public StringProducerUnit(RoboContext context, String id) { + super(String.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString("target", null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException("target"); - } - counter = new AtomicInteger(DEFAULT); + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString("target", null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException("target"); + } + counter = new AtomicInteger(DEFAULT); - } + } - @Override - public void onMessage(String message) { - if (message == null) { - System.out.println("No Message!"); - } else { - counter.incrementAndGet(); - getContext().getReference(target).sendMessage(message); - } - } + @Override + public void onMessage(String message) { + if (message == null) { + LOGGER.info("No Message!"); + } else { + counter.incrementAndGet(); + getContext().getReference(target).sendMessage(message); + } + } - @SuppressWarnings("unchecked") - @Override - public synchronized R onGetAttribute(AttributeDescriptor attribute) { - if (attribute.getAttributeName().equals("getNumberOfSentMessages") - && attribute.getAttributeType() == Integer.class) { - return (R) (Integer) counter.get(); - } - return null; - } + @SuppressWarnings("unchecked") + @Override + public synchronized R onGetAttribute(AttributeDescriptor attribute) { + if (attribute.getAttributeName().equals("getNumberOfSentMessages") + && attribute.getAttributeType() == Integer.class) { + return (R) (Integer) counter.get(); + } + return null; + } } From a60393604ee9e0a663922585f2ebef67d3600aa3 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Tue, 8 Oct 2024 17:48:06 +0200 Subject: [PATCH 07/13] [69] robo4j-socket-http update --- .../socket/http/util/RoboHttpUtils.java | 119 +++--- .../http/test/json/JsonCodecsTests.java | 8 +- .../GenericCodecConversionCyclesTests.java | 361 +++++++++--------- .../http/test/utils/ChannelBufferTests.java | 52 +-- .../test/utils/HttpMessageBuilderTests.java | 58 +-- .../utils/HttpPathConfigJsonBuilderTests.java | 8 +- .../http/test/utils/HttpPathUtilTests.java | 78 ++-- .../http/test/utils/InternalUtilTests.java | 9 +- .../socket/http/test/utils/JsonUtilTests.java | 87 +++-- .../http/test/utils/ReflectUtilTests.java | 259 ++++++------- 10 files changed, 524 insertions(+), 515 deletions(-) diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/RoboHttpUtils.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/RoboHttpUtils.java index 3b8b7c3a..b04cc6d9 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/RoboHttpUtils.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/RoboHttpUtils.java @@ -18,6 +18,8 @@ import com.robo4j.util.StringConstants; import com.robo4j.util.Utf8Constant; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Objects; @@ -28,73 +30,68 @@ * @author Miroslav Wengner (@miragemiko) */ public final class RoboHttpUtils { + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpUtils.class); - public static final String NEW_LINE_MAC = "\r"; - public static final String NEW_LINE_UNIX = "\n"; + public static final String NEW_LINE_MAC = "\r"; + public static final String NEW_LINE_UNIX = "\n"; - public static final int DEFAULT_PORT = 8042; - public static final int DEFAULT_UDP_PORT = 9042; - public static final String HTTP_PROPERTY_PROTOCOL = "protocol"; - public static final String PROPERTY_TARGET = "target"; - public static final String PROPERTY_HOST = "host"; - public static final String PROPERTY_SOCKET_PORT = "port"; - public static final String PROPERTY_CODEC_REGISTRY = "codecRegistry"; - public static final String PROPERTY_CODEC_PACKAGES = "packages"; - public static final String PROPERTY_UNIT_PATHS_CONFIG = "unitPathsConfig"; - public static final String PROPERTY_BUFFER_CAPACITY = "bufferCapacity"; - public static final String PROPERTY_BYTE_BUFFER = "byteBuffer"; - public static final String PROPERTY_TIMEOUT = "timeout"; + public static final int DEFAULT_PORT = 8042; + public static final int DEFAULT_UDP_PORT = 9042; + public static final String HTTP_PROPERTY_PROTOCOL = "protocol"; + public static final String PROPERTY_TARGET = "target"; + public static final String PROPERTY_HOST = "host"; + public static final String PROPERTY_SOCKET_PORT = "port"; + public static final String PROPERTY_CODEC_REGISTRY = "codecRegistry"; + public static final String PROPERTY_CODEC_PACKAGES = "packages"; + public static final String PROPERTY_UNIT_PATHS_CONFIG = "unitPathsConfig"; + public static final String PROPERTY_BUFFER_CAPACITY = "bufferCapacity"; + public static final String PROPERTY_BYTE_BUFFER = "byteBuffer"; + public static final String PROPERTY_TIMEOUT = "timeout"; - public static void decorateByNewLine(StringBuilder sb) { - sb.append(NEW_LINE_MAC).append(NEW_LINE_UNIX); - } + public static void decorateByNewLine(StringBuilder sb) { + sb.append(NEW_LINE_MAC).append(NEW_LINE_UNIX); + } - public static String correctLine(String line) { - return line == null ? StringConstants.EMPTY : line; - } + public static String correctLine(String line) { + return line == null ? StringConstants.EMPTY : line; + } - /** - * create host header note - * - * @param host - * desired host - * @param port - * default port is 80 - * @return host string - */ - public static String createHost(String host, Integer port) { - Objects.requireNonNull(host, "host not available"); - Objects.requireNonNull(host, "port not available"); - return new StringBuilder(host).append(Utf8Constant.UTF8_COLON).append(port).toString(); - } + /** + * create host header note + * + * @param host desired host + * @param port default port is 80 + * @return host string + */ + public static String createHost(String host, Integer port) { + Objects.requireNonNull(host, "host not available"); + Objects.requireNonNull(host, "port not available"); + return host + Utf8Constant.UTF8_COLON + port; + } - /** - * report time in moment M from start time - * - * @param clazz - * desired class - * @param message - * message - * @param start - * start time - */ - public static void printMeasuredTime(Class clazz, String message, long start) { - System.out.println(String.format("%s message: %s duration: %d%n", clazz.getSimpleName(), message, - System.currentTimeMillis() - start)); - } + /** + * report time in moment M from start time + * + * @param clazz desired class + * @param message message + * @param start start time + */ + public static void printMeasuredTime(Class clazz, String message, long start) { + LOGGER.info("{} message: {} duration: {}", clazz.getSimpleName(), message, System.currentTimeMillis() - start); + } - // TODO: 1/26/18 (miro - public static boolean validatePackages(String packages) { - if (packages == null) { - return false; - } - for (int i = 0; i < packages.length(); i++) { - char c = packages.charAt(i); - if (Character.isWhitespace(c)) { - return false; - } - } - return true; - } + // TODO: 1/26/18 (miro) review and simplify + public static boolean validatePackages(String packages) { + if (packages == null) { + return false; + } + for (int i = 0; i < packages.length(); i++) { + char c = packages.charAt(i); + if (Character.isWhitespace(c)) { + return false; + } + } + return true; + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java index b1c7475a..6ee92de1 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/json/JsonCodecsTests.java @@ -223,10 +223,6 @@ private static void printDecodedMessage(T decodedMessage) { LOGGER.debug("decodedMessage:{}", decodedMessage); } - private static void printJson(String resultJson) { - LOGGER.debug("resultJson:{}", resultJson); - } - private static Map getStringTestPersonMap() { TestPerson testPerson2 = new TestPerson(); testPerson2.setName("name2"); @@ -252,4 +248,8 @@ private static Map getStringTestPersonMap() { return personMap; } + private static void printJson(String resultJson) { + LOGGER.debug("json:{}", resultJson); + } + } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/codec/GenericCodecConversionCyclesTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/codec/GenericCodecConversionCyclesTests.java index 598fd6b9..b0be5da0 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/codec/GenericCodecConversionCyclesTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/codec/GenericCodecConversionCyclesTests.java @@ -20,196 +20,195 @@ import com.robo4j.socket.http.test.codec.NSBWithSimpleCollectionsTypesMessageCodec; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.Collections; -import java.util.HashMap; import java.util.LinkedHashMap; -import java.util.List; -import java.util.Map; -import static org.junit.jupiter.api.Assertions.assertArrayEquals; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class GenericCodecConversionCyclesTests { - - private NSBTypesTestMessageCodec fieldTypesMessageCodec; - private NSBWithSimpleCollectionsTypesMessageCodec collectionsTypesMessageCodec; - - @BeforeEach - void setUp() { - fieldTypesMessageCodec = new NSBTypesTestMessageCodec(); - collectionsTypesMessageCodec = new NSBWithSimpleCollectionsTypesMessageCodec(); - } - - @Test - void genericClassCycleFromObjectToJsonWithNullExtractionTest() { - int numberValue = 22; - boolean isActive = true; - String desiredJson = "{\"number\":" + numberValue + ",\"active\":" + isActive + "}"; - - NSBTypesTestMessage obj1 = new NSBTypesTestMessage(numberValue, null, isActive); - - String json = fieldTypesMessageCodec.encode(obj1); - assertEquals(desiredJson, json); - System.out.println("JSON: " + json); - } - - @Test - void genericClassCycleFromObjectToJsonExtractionTest() { - int numberValue = 22; - boolean isActive = true; - String message = "some messge"; - String desiredJson = "{\"number\":" + numberValue + ",\"message\":\"" + message + "\",\"active\":" + isActive - + "}"; - NSBTypesTestMessage obj1 = new NSBTypesTestMessage(numberValue, message, isActive); - - String json = fieldTypesMessageCodec.encode(obj1); - assertEquals(desiredJson, json); - System.out.println("JSON: " + json); - } - - @Test - void genericClassCycleFromObjectToJsonToObjectTest() { - - NSBTypesTestMessage obj1 = new NSBTypesTestMessage(22, "some messge", true); - - String json = fieldTypesMessageCodec.encode(obj1); - System.out.println("json: " + json); - NSBTypesTestMessage createdObj = fieldTypesMessageCodec.decode(json); - - assertEquals(obj1, createdObj); - System.out.println("Result: " + createdObj); - } - - @Test - void genericClassCycleFromObjectToJsonToObjectWithNulTest() { - NSBTypesTestMessage obj1 = new NSBTypesTestMessage(22, null, true); - - String json = fieldTypesMessageCodec.encode(obj1); - NSBTypesTestMessage createdObj = fieldTypesMessageCodec.decode(json); - - assertEquals(obj1, createdObj); - System.out.println("json: " + json); - System.out.println("Result: " + createdObj); - } - - @Test - void genericClassCycleFromObjectToJsonToObjectWithArrayTest() { - String desiredObjectToJson = "{\"number\":42,\"message\":\"no message\",\"active\":false,\"array\":[\"one\",\"two\"],\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}}"; - NSBWithSimpleCollectionsTypesMessage obj1 = new NSBWithSimpleCollectionsTypesMessage(); - obj1.setNumber(42); - obj1.setMessage("no message"); - obj1.setActive(false); - obj1.setArray(new String[] { "one", "two" }); - obj1.setList(Arrays.asList("text1", "text2")); - Map mapStrings = new HashMap<>(); - mapStrings.put("key1", "value1"); - mapStrings.put("key2", "value2"); - mapStrings.put("key3", "value3"); - obj1.setMap(Collections.singletonMap("key", "value")); - - String json = collectionsTypesMessageCodec.encode(obj1); - System.out.println("JSON:" + json); - NSBWithSimpleCollectionsTypesMessage createdObj = collectionsTypesMessageCodec.decode(json); - - assertEquals(desiredObjectToJson, json); - assertEquals("no message", createdObj.getMessage()); - assertTrue(!createdObj.getActive()); - System.out.println("createdObj: " + createdObj); - System.out.println("json: " + json); - - } - - @Test - void testJson() { - - TestPerson testPerson2 = new TestPerson(); - testPerson2.setName("name2"); - testPerson2.setValue(5); - - TestPerson testPerson111 = new TestPerson(); - testPerson111.setName("name111"); - testPerson111.setValue(42); - - TestPerson testPerson11 = new TestPerson(); - testPerson11.setName("name11"); - testPerson11.setValue(0); - testPerson11.setChild(testPerson111); - - TestPerson testPerson1 = new TestPerson(); - testPerson1.setName("name1"); - testPerson1.setValue(22); - testPerson1.setChild(testPerson11); - - Map personMap = new LinkedHashMap<>(); - personMap.put("person1", testPerson1); - personMap.put("person2", testPerson2); - - NSBWithSimpleCollectionsTypesMessage obj1 = new NSBWithSimpleCollectionsTypesMessage(); - obj1.setNumber(42); - obj1.setMessage("no message"); - obj1.setActive(false); - obj1.setArray(new String[] { "one", "two" }); - obj1.setList(Arrays.asList("text1", "text2")); - obj1.setMap(Collections.singletonMap("key", "value")); - obj1.setPersons(Arrays.asList(testPerson1, testPerson2)); - obj1.setPersonMap(personMap); - - String json = collectionsTypesMessageCodec.encode(obj1); - System.out.println("JSON: " + json); - } - - @Test - void collectionNSBWithSimpleCollectionsTypesMessageNestedObject() { - String json = "{\"number\":42,\"message\":\"no message\",\"active\":false,\"array\":[\"one\",\"two\"]," - + "\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}, " - + "\"persons\":[{\"name\":\"name1\",\"value\":22, \"child\":{\"name\":\"name11\",\"value\":0, " - + "\"child\":{\"name\":\"name111\",\"value\":42}}},{\"name\":\"name2\",\"value\":5}], " - + "\"personMap\":{\"key1\":\"value1\",\"key2,\":\"value2\"}}"; - - System.out.println("JSON: " + json); - NSBWithSimpleCollectionsTypesMessage createdObj = collectionsTypesMessageCodec.decode(json); - - List expectedList = Arrays.asList("text1", "text2"); - - TestPerson testPerson2 = new TestPerson(); - testPerson2.setName("name2"); - testPerson2.setValue(5); - - TestPerson testPerson111 = new TestPerson(); - testPerson111.setName("name111"); - testPerson111.setValue(42); - - TestPerson testPerson11 = new TestPerson(); - testPerson11.setName("name11"); - testPerson11.setValue(0); - testPerson11.setChild(testPerson111); - - TestPerson testPerson1 = new TestPerson(); - testPerson1.setName("name1"); - testPerson1.setValue(22); - testPerson1.setChild(testPerson11); - - Map expectedMap = Collections.singletonMap("key", "value"); - - assertEquals(Integer.valueOf(42), createdObj.getNumber()); - assertTrue(!createdObj.getActive()); - assertArrayEquals(new String[] { "one", "two" }, createdObj.getArray()); - assertEquals(expectedMap, createdObj.getMap()); - assertEquals(expectedList.size(), createdObj.getList().size()); - assertTrue(createdObj.getList().containsAll(expectedList)); - assertEquals("no message", createdObj.getMessage()); - assertEquals(testPerson1, createdObj.getPersons().get(0)); - assertEquals(testPerson2, createdObj.getPersons().get(1)); - assertEquals(testPerson11, createdObj.getPersons().get(0).getChild()); - - System.out.println("createdObj: " + createdObj); - } - + private static final Logger LOGGER = LoggerFactory.getLogger(GenericCodecConversionCyclesTests.class); + private NSBTypesTestMessageCodec fieldTypesMessageCodec; + private NSBWithSimpleCollectionsTypesMessageCodec collectionsTypesMessageCodec; + + @BeforeEach + void setUp() { + fieldTypesMessageCodec = new NSBTypesTestMessageCodec(); + collectionsTypesMessageCodec = new NSBWithSimpleCollectionsTypesMessageCodec(); + } + + @Test + void genericClassCycleFromObjectToJsonWithNullExtractionTest() { + int numberValue = 22; + boolean isActive = true; + String desiredJson = "{\"number\":" + numberValue + ",\"active\":" + isActive + "}"; + + NSBTypesTestMessage obj1 = new NSBTypesTestMessage(numberValue, null, isActive); + + String json = fieldTypesMessageCodec.encode(obj1); + assertEquals(desiredJson, json); + printJson(json); + } + + @Test + void genericClassCycleFromObjectToJsonExtractionTest() { + int numberValue = 22; + boolean isActive = true; + String message = "some messge"; + String desiredJson = "{\"number\":" + numberValue + ",\"message\":\"" + message + "\",\"active\":" + isActive + + "}"; + NSBTypesTestMessage obj1 = new NSBTypesTestMessage(numberValue, message, isActive); + + String json = fieldTypesMessageCodec.encode(obj1); + assertEquals(desiredJson, json); + printJson(json); + } + + @Test + void genericClassCycleFromObjectToJsonToObjectTest() { + + NSBTypesTestMessage obj1 = new NSBTypesTestMessage(22, "some messge", true); + + String json = fieldTypesMessageCodec.encode(obj1); + NSBTypesTestMessage createdObj = fieldTypesMessageCodec.decode(json); + + printJson(json); + LOGGER.info("Result: {}", createdObj); + assertEquals(obj1, createdObj); + } + + @Test + void genericClassCycleFromObjectToJsonToObjectWithNulTest() { + var obj1 = new NSBTypesTestMessage(22, null, true); + var json = fieldTypesMessageCodec.encode(obj1); + var createdObj = fieldTypesMessageCodec.decode(json); + + printJson(json); + printObject(createdObj); + + assertEquals(obj1, createdObj); + } + + @Test + void genericClassCycleFromObjectToJsonToObjectWithArrayTest() { + String desiredObjectToJson = "{\"number\":42,\"message\":\"no message\",\"active\":false,\"array\":[\"one\",\"two\"],\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}}"; + NSBWithSimpleCollectionsTypesMessage obj1 = new NSBWithSimpleCollectionsTypesMessage(); + obj1.setNumber(42); + obj1.setMessage("no message"); + obj1.setActive(false); + obj1.setArray(new String[]{"one", "two"}); + obj1.setList(Arrays.asList("text1", "text2")); + obj1.setMap(Collections.singletonMap("key", "value")); + + String json = collectionsTypesMessageCodec.encode(obj1); + var createdObj = collectionsTypesMessageCodec.decode(json); + + printJson(json); + printObject(createdObj); + + assertEquals(desiredObjectToJson, json); + assertEquals("no message", createdObj.getMessage()); + assertFalse(createdObj.getActive()); + + } + + @Test + void testJson() { + TestPerson testPerson2 = new TestPerson(); + testPerson2.setName("name2"); + testPerson2.setValue(5); + + TestPerson testPerson111 = new TestPerson(); + testPerson111.setName("name111"); + testPerson111.setValue(42); + + TestPerson testPerson11 = new TestPerson(); + testPerson11.setName("name11"); + testPerson11.setValue(0); + testPerson11.setChild(testPerson111); + + TestPerson testPerson1 = new TestPerson(); + testPerson1.setName("name1"); + testPerson1.setValue(22); + testPerson1.setChild(testPerson11); + + var personMap = new LinkedHashMap(); + personMap.put("person1", testPerson1); + personMap.put("person2", testPerson2); + + var obj1 = new NSBWithSimpleCollectionsTypesMessage(); + obj1.setNumber(42); + obj1.setMessage("no message"); + obj1.setActive(false); + obj1.setArray(new String[]{"one", "two"}); + obj1.setList(Arrays.asList("text1", "text2")); + obj1.setMap(Collections.singletonMap("key", "value")); + obj1.setPersons(Arrays.asList(testPerson1, testPerson2)); + obj1.setPersonMap(personMap); + String json = collectionsTypesMessageCodec.encode(obj1); + + printJson(json); + } + + @Test + void collectionNSBWithSimpleCollectionsTypesMessageNestedObject() { + String json = "{\"number\":42,\"message\":\"no message\",\"active\":false,\"array\":[\"one\",\"two\"]," + + "\"list\":[\"text1\",\"text2\"],\"map\":{\"key\":\"value\"}, " + + "\"persons\":[{\"name\":\"name1\",\"value\":22, \"child\":{\"name\":\"name11\",\"value\":0, " + + "\"child\":{\"name\":\"name111\",\"value\":42}}},{\"name\":\"name2\",\"value\":5}], " + + "\"personMap\":{\"key1\":\"value1\",\"key2,\":\"value2\"}}"; + + var createdObj = collectionsTypesMessageCodec.decode(json); + + var expectedList = Arrays.asList("text1", "text2"); + + TestPerson testPerson2 = new TestPerson(); + testPerson2.setName("name2"); + testPerson2.setValue(5); + + TestPerson testPerson111 = new TestPerson(); + testPerson111.setName("name111"); + testPerson111.setValue(42); + + TestPerson testPerson11 = new TestPerson(); + testPerson11.setName("name11"); + testPerson11.setValue(0); + testPerson11.setChild(testPerson111); + + TestPerson testPerson1 = new TestPerson(); + testPerson1.setName("name1"); + testPerson1.setValue(22); + testPerson1.setChild(testPerson11); + + var expectedMap = Collections.singletonMap("key", "value"); + + printJson(json); + printObject(createdObj); + + assertEquals(Integer.valueOf(42), createdObj.getNumber()); + assertFalse(createdObj.getActive()); + assertArrayEquals(new String[]{"one", "two"}, createdObj.getArray()); + assertEquals(expectedMap, createdObj.getMap()); + assertEquals(expectedList.size(), createdObj.getList().size()); + assertTrue(createdObj.getList().containsAll(expectedList)); + assertEquals("no message", createdObj.getMessage()); + assertEquals(testPerson1, createdObj.getPersons().get(0)); + assertEquals(testPerson2, createdObj.getPersons().get(1)); + assertEquals(testPerson11, createdObj.getPersons().get(0).getChild()); + } + + static void printJson(String json) { + LOGGER.info("JSON: {}", json); + } + + private static void printObject(Object obj) { + LOGGER.info("Object: {}", obj); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ChannelBufferTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ChannelBufferTests.java index 09df8272..25c3063b 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ChannelBufferTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ChannelBufferTests.java @@ -17,8 +17,8 @@ package com.robo4j.socket.http.test.utils; import org.junit.jupiter.api.Test; - -import java.util.regex.Matcher; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static com.robo4j.socket.http.util.ChannelBufferUtils.RESPONSE_SPRING_PATTERN; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -30,22 +30,23 @@ * @author Miroslav Wengner (@miragemiko) */ class ChannelBufferTests { - private static int GROUP_COUNT = 3; + private static final Logger LOGGER = LoggerFactory.getLogger(ChannelBufferTests.class); + private static final int GROUP_COUNT = 3; private static final String JSON_MESSAGE = "{\"content\":\"No Params\",\"number\":11}"; private static final String JSON_ARRAY_MESSAGE = "[{\"id\":\"stringConsumer\",\"state\":\"STARTED\"},{\"id\":\"httpServer\",\"state\":\"STARTED\"}]"; @Test - void springResponsePatternTest(){ - String message = "HTTP/1.1 200 \r\n" + + void springResponsePatternTest() { + var message = "HTTP/1.1 200 \r\n" + "Content-Type: application/json;charset=UTF-8\r\n" + "Transfer-Encoding: chunked\r\n" + "Date: Mon, 05 Mar 2018 21:37:27 GMT\r\n" + "\r\n" + - "23\r\n" + JSON_MESSAGE +"\r\n"; - String[] headerBody = message.split("\r\n\r\n"); - Matcher matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); - if(matcher.find()){ - System.out.println("json: " + matcher.group(2)); + "23\r\n" + JSON_MESSAGE + "\r\n"; + var headerBody = message.split("\r\n\r\n"); + var matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); + if (matcher.find()) { + printJson(matcher.group(2)); assertEquals(GROUP_COUNT, matcher.groupCount()); assertEquals(JSON_MESSAGE, matcher.group(2)); } @@ -53,35 +54,38 @@ void springResponsePatternTest(){ @Test - void jsonResponseStandardTest(){ - String message = "HTTP/1.1 200 \r\n" + + void jsonResponseStandardTest() { + var message = "HTTP/1.1 200 \r\n" + "Content-Type: application/json;charset=UTF-8\r\n" + "Transfer-Encoding: chunked\r\n" + "Date: Mon, 05 Mar 2018 21:37:27 GMT\r\n" + - "\r\n" + JSON_MESSAGE +"\r\n"; + "\r\n" + JSON_MESSAGE + "\r\n"; - String[] headerBody = message.split("\r\n\r\n"); - Matcher matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); - if(matcher.find()){ - System.out.println("json: " + matcher.group(2)); + var headerBody = message.split("\r\n\r\n"); + var matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); + if (matcher.find()) { + printJson(matcher.group(2)); assertEquals(JSON_MESSAGE, matcher.group(2)); } assertEquals(GROUP_COUNT, matcher.groupCount()); } @Test - void jsonArrayResponseMessage(){ - String message = "HTTP/1.1 200 \r\n" + + void jsonArrayResponseMessage() { + var message = "HTTP/1.1 200 \r\n" + "Date: Mon, 05 Mar 2018 21:37:27 GMT\r\n" + - "\r\n" + JSON_ARRAY_MESSAGE +"\r\n"; + "\r\n" + JSON_ARRAY_MESSAGE + "\r\n"; - String[] headerBody = message.split("\r\n\r\n"); - Matcher matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); - if(matcher.find()){ - System.out.println("json: " + matcher.group(2)); + var headerBody = message.split("\r\n\r\n"); + var matcher = RESPONSE_SPRING_PATTERN.matcher(headerBody[1]); + if (matcher.find()) { + printJson(matcher.group(2)); assertEquals(JSON_ARRAY_MESSAGE, matcher.group(2)); } assertEquals(GROUP_COUNT, matcher.groupCount()); } + private static void printJson(String resultJson) { + LOGGER.debug("json:{}", resultJson); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpMessageBuilderTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpMessageBuilderTests.java index 74a455a8..ed0adf08 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpMessageBuilderTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpMessageBuilderTests.java @@ -24,6 +24,8 @@ import com.robo4j.socket.http.util.HttpMessageBuilder; import com.robo4j.socket.http.util.RoboHttpUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static com.robo4j.socket.http.provider.DefaultValuesProvider.BASIC_HEADER_MAP; import static org.junit.jupiter.api.Assertions.assertNotNull; @@ -34,38 +36,40 @@ * @author Miroslav Wengner (@miragemiko) */ class HttpMessageBuilderTests { + private static final Logger LOGGER = LoggerFactory.getLogger(HttpMessageBuilderTests.class); - @Test - void getRequestTest() { - HttpMessageBuilder getMessageBuilder = getDefaultMessageBuilderByMethod(HttpMethod.GET); + @Test + void getRequestTest() { + var getMessageBuilder = getDefaultMessageBuilderByMethod(HttpMethod.GET); - final String getMessage = getMessageBuilder.build(); - assertNotNull(getMessage); - assertTrue(getMessage.contains("GET / HTTP/1.1")); - assertTrue(getMessage.contains("host: localhost")); - System.out.println("toMessage: " + getMessage); - } + var message = getMessageBuilder.build(); - @Test - void postRequestTest() { - String message = "magic"; - HttpMessageBuilder postMessageBuilder = getDefaultMessageBuilderByMethod(HttpMethod.POST); - postMessageBuilder.addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(message.length())); + LOGGER.info("toMessage: {}", message); + assertNotNull(message); + assertTrue(message.contains("GET / HTTP/1.1")); + assertTrue(message.contains("host: localhost")); + } - final String postRequest = postMessageBuilder.build(message); + @Test + void postRequestTest() { + String message = "magic"; + HttpMessageBuilder postMessageBuilder = getDefaultMessageBuilderByMethod(HttpMethod.POST); + postMessageBuilder.addHeaderElement(HttpHeaderFieldNames.CONTENT_LENGTH, String.valueOf(message.length())); - assertNotNull(postRequest); - assertTrue(postRequest.contains("POST / HTTP/1.1")); - assertTrue(postRequest.contains("host: localhost")); - assertTrue(postRequest.contains("content-length: 5")); - assertTrue(postRequest.contains(message)); - } + final String postRequest = postMessageBuilder.build(message); + assertNotNull(postRequest); + assertTrue(postRequest.contains("POST / HTTP/1.1")); + assertTrue(postRequest.contains("host: localhost")); + assertTrue(postRequest.contains("content-length: 5")); + assertTrue(postRequest.contains(message)); + } - private HttpMessageBuilder getDefaultMessageBuilderByMethod(HttpMethod method) { - HttpRequestDenominator getDenominator = new HttpRequestDenominator(method, HttpVersion.HTTP_1_1); - return HttpMessageBuilder.Build().setDenominator(getDenominator) - .addHeaderElement(HttpHeaderFieldNames.HOST, RoboHttpUtils.createHost("localhost", ProtocolType.HTTP.getPort())) - .addHeaderElements(BASIC_HEADER_MAP); - } + + private HttpMessageBuilder getDefaultMessageBuilderByMethod(HttpMethod method) { + HttpRequestDenominator getDenominator = new HttpRequestDenominator(method, HttpVersion.HTTP_1_1); + return HttpMessageBuilder.Build().setDenominator(getDenominator) + .addHeaderElement(HttpHeaderFieldNames.HOST, RoboHttpUtils.createHost("localhost", ProtocolType.HTTP.getPort())) + .addHeaderElements(BASIC_HEADER_MAP); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathConfigJsonBuilderTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathConfigJsonBuilderTests.java index 69f5e561..077b9bbd 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathConfigJsonBuilderTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathConfigJsonBuilderTests.java @@ -19,6 +19,8 @@ import com.robo4j.socket.http.HttpMethod; import com.robo4j.socket.http.util.HttpPathConfigJsonBuilder; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.Collections; @@ -26,14 +28,14 @@ import static org.junit.jupiter.api.Assertions.assertEquals; /** - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ class HttpPathConfigJsonBuilderTests { + private static final Logger LOGGER = LoggerFactory.getLogger(HttpPathConfigJsonBuilderTests.class); @Test - void simpleConfigurationTest(){ + void simpleConfigurationTest() { String expectedJson = "[{\"roboUnit\":\"roboUnit1\",\"method\":\"GET\",\"callbacks\":[\"filter1\",\"filter2\"]}," + "{\"roboUnit\":\"roboUnit2\",\"method\":\"POST\",\"callbacks\":[]},{\"roboUnit\":\"roboUnit3\",\"method\":\"GET\",\"callbacks\":[]}]"; @@ -45,7 +47,7 @@ void simpleConfigurationTest(){ String resultJson = builder.build(); - System.out.println("resultJson: " + resultJson); + LOGGER.info("resultJson: {}", resultJson); assertEquals(expectedJson, resultJson); } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathUtilTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathUtilTests.java index dfc3d89f..74960e57 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathUtilTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/HttpPathUtilTests.java @@ -17,7 +17,6 @@ package com.robo4j.socket.http.test.utils; import com.robo4j.socket.http.HttpMethod; -import com.robo4j.socket.http.dto.ClassGetSetDTO; import com.robo4j.socket.http.dto.HttpPathMethodDTO; import com.robo4j.socket.http.dto.PathAttributeDTO; import com.robo4j.socket.http.test.units.config.PropertyListBuilder; @@ -26,42 +25,32 @@ import com.robo4j.socket.http.util.ReflectUtils; import com.robo4j.util.StringConstants; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.Arrays; -import java.util.Collections; -import java.util.HashMap; -import java.util.HashSet; -import java.util.List; -import java.util.Map; -import java.util.Set; +import java.util.*; import static com.robo4j.socket.http.util.HttpPathUtils.ATTRIBUTES_PATH_VALUE; -import static org.junit.jupiter.api.Assertions.assertArrayEquals; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class HttpPathUtilTests { + private static final Logger LOGGER = LoggerFactory.getLogger(HttpPathUtilTests.class); - //String roboUnit; - // private HttpMethod method; - // private List callbacks; + @SuppressWarnings("unchecked") + @Test + public void convertPathMethodToJson() { - @SuppressWarnings("unchecked") - @Test - public void convertPathMethodToJson() { - - //@formatter:off - final String expectedResult = "[{\"roboUnit\":\"imageController\",\"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + + //@formatter:off + final var expectedResult = "[{\"roboUnit\":\"imageController\",\"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + "{\"roboUnit\":\"imageController\",\"method\":\"GET\",\"callbacks\":[\"callbackGETController\"]}," + "{\"roboUnit\":\"cameraController\",\"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + "{\"roboUnit\":\"cameraController\",\"method\":\"GET\",\"callbacks\":[\"callbackGETController\"]}," + "{\"roboUnit\":\"emptyController\",\"method\":\"GET\"}]"; - List pathMethodList = PropertyListBuilder.Builder() + final var pathMethodList = PropertyListBuilder.Builder() .add(createPathMethodDTO("imageController", "POST","callbackPOSTController")) .add(createPathMethodDTO("imageController", "GET","callbackGETController")) .add(createPathMethodDTO("cameraController", "POST","callbackPOSTController")) @@ -69,9 +58,9 @@ public void convertPathMethodToJson() { .add(createPathMethodDTO("emptyController", "GET")) .build(); - String result = JsonUtil.getJsonByPathMethodList(pathMethodList); + var result = JsonUtil.getJsonByPathMethodList(pathMethodList); - System.out.println("result: " + result); + printInfo(result); assertNotNull(result); assertEquals(expectedResult, result); @@ -79,18 +68,18 @@ public void convertPathMethodToJson() { @Test void parseJsonArrayPathMethodToList(){ - final int observedElement = 2; - final String jsonArray = "[{\"roboUnit\":\"imageController\", \"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + + final var observedElement = 2; + final var jsonArray = "[{\"roboUnit\":\"imageController\", \"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + "{\"roboUnit\":\"imageController\",\"method\":\"GET\",\"callbacks\":[\"callbackGETController\"]}," + "{\"roboUnit\":\"cameraController\",\"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + "{\"roboUnit\":\"cameraController\",\"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}," + "{\"roboUnit\":\"cameraController\",\"method\":\"GET\",\"callbacks\":[\"callbackGETController\"]}," + "{\"roboUnit\":\"emptyController\",\"method\":\"GET\"}]"; - HttpPathMethodDTO duplicate = new HttpPathMethodDTO("cameraController", HttpMethod.POST, Collections.singletonList("callbackPOSTController")); - List result = JsonUtil.toListFromJsonArray(HttpPathMethodDTO.class, jsonArray); + var duplicate = new HttpPathMethodDTO("cameraController", HttpMethod.POST, Collections.singletonList("callbackPOSTController")); + var result = JsonUtil.toListFromJsonArray(HttpPathMethodDTO.class, jsonArray); - System.out.println("result: " + result); + printInfo(result); assertNotNull(result); assertEquals(6, result.size()); @@ -103,7 +92,7 @@ void parseJsonArrayPathMethodToList(){ @Test void parseJsonPathMethod(){ - List jsonList = Arrays.asList( + final var jsonList = Arrays.asList( "{\"roboUnit\":\"imageController\", \"method\":\"POST\", \"callbacks\":[\"callbackPOSTController\"]}", "{\"roboUnit\":\"imageController\", \"method\" : \"POST\", \"callbacks\" : [ \"callbackPOSTController\" ]}", "{ \"roboUnit\": \"imageController\", \"method\": \"POST\", \"callbacks\" :[\"callbackPOSTController\"] }" @@ -120,7 +109,7 @@ void parseJsonPathMethod(){ @Test void parseFullPathMethod() { - List jsonList = Arrays.asList( + final var jsonList = Arrays.asList( "{\"roboUnit\":\"\", \"method\":\"POST\", \"callbacks\":[\"callbackPOSTController\"]}", "{\"roboUnit\":\"imageController\", \"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}", "{\"roboUnit\":\"imageController\", \"method\":\"POST\",\"callbacks\":[\"callbackPOSTController\"]}", @@ -129,7 +118,7 @@ void parseFullPathMethod() { jsonList.forEach(json -> { HttpPathMethodDTO pathMethod = JsonUtil.getPathMethodByJson(json); - System.out.println("pathMethod: " + pathMethod); + printInfo(pathMethod); assertNotNull(pathMethod, json); assertNotNull(pathMethod.getRoboUnit(), json); assertTrue(pathMethod.getRoboUnit().isEmpty() ? @@ -139,33 +128,38 @@ void parseFullPathMethod() { @Test void parseGetRequestWithAttributes(){ - String path = "/units/controller?attributes=number,counter"; - Map> expectedMap = new HashMap<>(); - Set expectedAttributesValues = new HashSet<>(); - String attributeName = "attributes"; + var path = "/units/controller?attributes=number,counter"; + var expectedMap = new HashMap>(); + var expectedAttributesValues = new HashSet(); + var attributeName = "attributes"; + expectedAttributesValues.add("number"); expectedAttributesValues.add("counter"); expectedMap.put(ATTRIBUTES_PATH_VALUE, expectedAttributesValues); HttpPathUtils.extractAttributesByPath(path); - Map> attributeMap = HttpPathUtils.extractAttributesByPath(path); + var attributeMap = HttpPathUtils.extractAttributesByPath(path); + assertArrayEquals(attributeMap.get(attributeName).toArray(), expectedMap.get(attributeName).toArray()); } @Test void createJsonArrayByList(){ + var attributeDTO = new PathAttributeDTO("number", "42"); - PathAttributeDTO attributeDTO = new PathAttributeDTO("number", "42"); + var descriptorMap = ReflectUtils.getFieldsTypeMap(PathAttributeDTO.class); - Map descriptorMap = ReflectUtils.getFieldsTypeMap(PathAttributeDTO.class); - - System.out.println("result: " + JsonUtil.toJson(descriptorMap, attributeDTO)); + printInfo(JsonUtil.toJson(descriptorMap, attributeDTO)); } private HttpPathMethodDTO createPathMethodDTO(String... args) { - List properties = args.length > 2 ? Collections.singletonList(args[2]) : null; + var properties = args.length > 2 ? Collections.singletonList(args[2]) : null; return new HttpPathMethodDTO(args[0], HttpMethod.getByName(args[1]), properties); } + private static void printInfo(T result) { + LOGGER.info("result: {}",result); + } + } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/InternalUtilTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/InternalUtilTests.java index f248ad91..46530390 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/InternalUtilTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/InternalUtilTests.java @@ -18,6 +18,8 @@ import com.robo4j.socket.http.util.SystemPropertyUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; @@ -29,12 +31,13 @@ * @author Miro Wengner (@miragemiko) */ class InternalUtilTests { + private static final Logger LOGGER = LoggerFactory.getLogger(InternalUtilTests.class); @Test - void testSeparator(){ - String separator = SystemPropertyUtils.get("line.separator", "\n\n"); - System.out.println("Separator: " + Arrays.asList(separator.toCharArray())); + void testSeparator() { + var separator = SystemPropertyUtils.get("line.separator", "\n\n"); + LOGGER.info("Separator: {}", Arrays.asList(separator.toCharArray())); assertNotNull(separator.toCharArray()); } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/JsonUtilTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/JsonUtilTests.java index 737d083f..2667c614 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/JsonUtilTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/JsonUtilTests.java @@ -20,16 +20,14 @@ import com.robo4j.socket.http.dto.ResponseUnitDTO; import com.robo4j.socket.http.util.JsonUtil; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.Collections; import java.util.HashMap; -import java.util.List; -import java.util.Map; -import static org.junit.jupiter.api.Assertions.assertArrayEquals; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.*; /** * json related utils tests @@ -38,53 +36,58 @@ * @author Miroslav Wengner (@miragemiko) */ class JsonUtilTests { + private static final Logger LOGGER = LoggerFactory.getLogger(JsonUtilTests.class); - @Test - void jsonToListTest() { + @Test + void jsonToListTest() { - String json = "[{\"id\":\"unit1\",\"state\":\"INITIALIZED\"}," + "{\"id\":\"unit2\",\"state\":\"STARTED\"}, " - + "{\"id\":\"unit3\",\"state\":\"FAILED\"}]"; + var json = "[{\"id\":\"unit1\",\"state\":\"INITIALIZED\"}," + "{\"id\":\"unit2\",\"state\":\"STARTED\"}, " + + "{\"id\":\"unit3\",\"state\":\"FAILED\"}]"; - List expectedResult = Arrays.asList(new ResponseUnitDTO("unit1", LifecycleState.INITIALIZED), - new ResponseUnitDTO("unit2", LifecycleState.STARTED), - new ResponseUnitDTO("unit3", LifecycleState.FAILED)); - List result = JsonUtil.jsonToList(ResponseUnitDTO.class, json); + var expectedResult = Arrays.asList(new ResponseUnitDTO("unit1", LifecycleState.INITIALIZED), + new ResponseUnitDTO("unit2", LifecycleState.STARTED), + new ResponseUnitDTO("unit3", LifecycleState.FAILED)); + var result = JsonUtil.jsonToList(ResponseUnitDTO.class, json); - assertNotNull(result); - assertArrayEquals(expectedResult.toArray(), result.toArray()); - } + assertNotNull(result); + assertArrayEquals(expectedResult.toArray(), result.toArray()); + } - @Test - void jsonToListEmptyTest() { - String json = "[]"; - List result = JsonUtil.jsonToList(ResponseUnitDTO.class, json); + @Test + void jsonToListEmptyTest() { + var json = "[]"; + var result = JsonUtil.jsonToList(ResponseUnitDTO.class, json); - assertNotNull(result); - assertArrayEquals(Collections.emptyList().toArray(), result.toArray()); - } + assertNotNull(result); + assertArrayEquals(Collections.emptyList().toArray(), result.toArray()); + } - @Test - void mapToJsonTest() { - String expectedJson = "{\"key1\":\"value1\",\"key2\":\"value2\"}"; - Map testMap = new HashMap<>(); - testMap.put("key1", "value1"); - testMap.put("key2", "value2"); + @Test + void mapToJsonTest() { + var expectedJson = "{\"key1\":\"value1\",\"key2\":\"value2\"}"; + var testMap = new HashMap(); + testMap.put("key1", "value1"); + testMap.put("key2", "value2"); - String result = JsonUtil.toJsonMap(testMap); - System.out.println("result: " + result); + var result = JsonUtil.toJsonMap(testMap); - assertNotNull(result); - assertEquals(result, expectedJson); - } + printInfo(result); + assertNotNull(result); + assertEquals(result, expectedJson); + } - @Test - void mapToJsonEmptyTest() { - String expectedJson = "{}"; + @Test + void mapToJsonEmptyTest() { + var expectedJson = "{}"; - String result = JsonUtil.toJsonMap(new HashMap<>()); - System.out.println("result: " + result); + var result = JsonUtil.toJsonMap(new HashMap<>()); - assertNotNull(result); - assertEquals(result, expectedJson); - } + printInfo(result); + assertNotNull(result); + assertEquals(result, expectedJson); + } + + private static void printInfo(String result) { + LOGGER.info("result: {}", result); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ReflectUtilTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ReflectUtilTests.java index 0862b1e9..e4f213a2 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ReflectUtilTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/utils/ReflectUtilTests.java @@ -25,18 +25,12 @@ import com.robo4j.socket.http.util.ReflectUtils; import com.robo4j.socket.http.util.RoboReflectException; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collections; -import java.util.HashMap; -import java.util.List; -import java.util.Map; +import java.util.*; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertThrows; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; /** @@ -44,162 +38,171 @@ * @author Miroslav Wengner (@miragemiko) */ class ReflectUtilTests { + private static final Logger LOGGER = LoggerFactory.getLogger(ReflectUtilTests.class); - @Test - void objectWithEnumToJson() { - final String expectedJson = "{\"command\":\"MOVE\",\"desc\":\"some description\"}"; - TestCommand command = new TestCommand(); - command.setCommand(TestCommandEnum.MOVE); - command.setDesc("some description"); - - final String result = ReflectUtils.createJson(command); + @Test + void objectWithEnumToJson() { + final var expectedJson = "{\"command\":\"MOVE\",\"desc\":\"some description\"}"; + var command = new TestCommand(); + command.setCommand(TestCommandEnum.MOVE); + command.setDesc("some description"); - System.out.println("result: " + result); - assertNotNull(result); - assertEquals(expectedJson, result); + var result = ReflectUtils.createJson(command); - } + printInfo(result); + assertNotNull(result); + assertEquals(expectedJson, result); + } - @Test - void objectWithEnumListToJson() { + @Test + void objectWithEnumListToJson() { - final String expectedJson = "{\"commands\":[\"MOVE\",\"STOP\",\"BACK\"],\"desc\":\"commands description\"}"; - TestCommandList commands = new TestCommandList(); - commands.setCommands(Arrays.asList(TestCommandEnum.MOVE, TestCommandEnum.STOP, TestCommandEnum.BACK)); - commands.setDesc("commands description"); + final var expectedJson = "{\"commands\":[\"MOVE\",\"STOP\",\"BACK\"],\"desc\":\"commands description\"}"; + var commands = new TestCommandList(); + commands.setCommands(Arrays.asList(TestCommandEnum.MOVE, TestCommandEnum.STOP, TestCommandEnum.BACK)); + commands.setDesc("commands description"); - final String result = ReflectUtils.createJson(commands); - System.out.println("result: " + result); + final String result = ReflectUtils.createJson(commands); - assertNotNull(result); - assertEquals(expectedJson, result); + printInfo(result); + assertNotNull(result); + assertEquals(expectedJson, result); - } + } - @Test - void pathAttributesListToJsonTest() { - final String expectedJson = "{\"attributes\":[{\"name\":\"name\",\"value\":\"java.lang.String\"},{\"name\":\"values\",\"value\":\"java.util.HashMap\"}]}"; - PathAttributeListDTO attributes = new PathAttributeListDTO(); - attributes.addAttribute(new PathAttributeDTO("name", "java.lang.String")); - attributes.addAttribute(new PathAttributeDTO("values", "java.util.HashMap")); - String result = ReflectUtils.createJson(attributes); - System.out.println("result:" + result); - assertEquals(expectedJson, result); - } + @Test + void pathAttributesListToJsonTest() { + final var expectedJson = "{\"attributes\":[{\"name\":\"name\",\"value\":\"java.lang.String\"},{\"name\":\"values\",\"value\":\"java.util.HashMap\"}]}"; + var attributes = new PathAttributeListDTO(); + attributes.addAttribute(new PathAttributeDTO("name", "java.lang.String")); + attributes.addAttribute(new PathAttributeDTO("values", "java.util.HashMap")); - @Test - void serverAttributesResponse(){ - ResponseAttributeListDTO tmpAttr = new ResponseAttributeListDTO(); + String result = ReflectUtils.createJson(attributes); - tmpAttr.setType("Type"); - tmpAttr.setId("ID"); - //String roboUnit, HttpMethod method, List callbacks - tmpAttr.addValue(new HttpPathMethodDTO("testUnit", HttpMethod.GET, Collections.singletonList("test"))); - String result = ReflectUtils.createJson(tmpAttr); - System.out.println("result:" + result); + printInfo(result); + assertEquals(expectedJson, result); + } - } + @Test + void serverAttributesResponse() { + var tmpAttr = new ResponseAttributeListDTO(); + tmpAttr.setType("Type"); + tmpAttr.setId("ID"); + //String roboUnit, HttpMethod method, List callbacks + tmpAttr.addValue(new HttpPathMethodDTO("testUnit", HttpMethod.GET, Collections.singletonList("test"))); + String result = ReflectUtils.createJson(tmpAttr); + + printInfo(result); + } - @Test - void complexObjectToJsonTest(){ - String expectedJson = "{\"name\":\"object\",\"value\":42,\"textList\":[\"one\",\"two\"],\"dictionary\":{\"one\":\"one1\",\"two\":\"two2\"},\"attributes\":{\"one\":{\"name\":\"name\",\"value\":\"test name\"},\"two\":{\"name\":\"value\",\"value\":\"42\"}}}"; - TestListMapValues obj = new TestListMapValues(); + @Test + void complexObjectToJsonTest() { + final var expectedJson = "{\"name\":\"object\",\"value\":42,\"textList\":[\"one\",\"two\"],\"dictionary\":{\"one\":\"one1\",\"two\":\"two2\"},\"attributes\":{\"one\":{\"name\":\"name\",\"value\":\"test name\"},\"two\":{\"name\":\"value\",\"value\":\"42\"}}}"; + var obj = new TestListMapValues(); obj.setName("object"); obj.setValue(42); obj.setTextList(Arrays.asList("one", "two")); - Map textMap = new HashMap<>(); + var textMap = new HashMap(); textMap.put("one", "one1"); textMap.put("two", "two2"); obj.setDictionary(textMap); - Map attributes = new HashMap<>(); + var attributes = new HashMap(); attributes.put("one", new PathAttributeDTO("name", "test name")); attributes.put("two", new PathAttributeDTO("value", "42")); obj.setAttributes(attributes); - String result = ReflectUtils.createJson(obj); - System.out.println("result:"+result); + var result = ReflectUtils.createJson(obj); + + printInfo(result); assertEquals(expectedJson, result); } @Test void createJsonByStringListCollectionTest() { - String expectedJson = "[\"One\",\"Two\"]"; - List list = new ArrayList<>(); + final var expectedJson = "[\"One\",\"Two\"]"; + var list = new ArrayList(); list.add("One"); list.add("Two"); String result = ReflectUtils.createJson(list); - System.out.println("result: " + result); + + printInfo(result); assertEquals(expectedJson, result); + } + + @Test + void createJsonByNumberListCollectionTest() { + final var expectedJson = "[1,2]"; + var list = new ArrayList(); + list.add(1); + list.add(2); + + var result = ReflectUtils.createJson(list); + + printInfo(result); + assertEquals(expectedJson, result); + + } + + @Test + void createJsonByObjectListCollectionTest() { + + Throwable exception = assertThrows(RoboReflectException.class, () -> { + List list = new ArrayList<>(); + list.add(new PathAttributeDTO("one", "1")); + list.add(new PathAttributeDTO("two", "2")); + ReflectUtils.createJson(list); + }); + + assertTrue(exception.getMessage().startsWith("object getter value")); + } + + @Test + void createJsonByStringMapCollection() { + final var expectedString = "{\"one\":\"1\",\"two\":\"2\"}"; + var map = new HashMap(); + map.put("one", "1"); + map.put("two", "2"); + + var result = ReflectUtils.createJson(map); + + printInfo(result); + assertEquals(expectedString, result); + + } + + @Test + void createJsonByNumberMapCollection() { + final var expectedString = "{\"one\":1,\"two\":2}"; + var map = new HashMap(); + map.put("one", 1); + map.put("two", 2); + var result = ReflectUtils.createJson(map); + + printInfo(result); + assertEquals(expectedString, result); } - @Test - void createJsonByNumberListCollectionTest() { - String expectedJson = "[1,2]"; - List list = new ArrayList<>(); - list.add(1); - list.add(2); - - String result = ReflectUtils.createJson(list); - System.out.println("result: " + result); - assertEquals(expectedJson, result); - - } - - @Test - void createJsonByObjectListCollectionTest(){ - - Throwable exception = assertThrows(RoboReflectException.class, () -> { - List list = new ArrayList<>(); - list.add(new PathAttributeDTO("one", "1")); - list.add(new PathAttributeDTO("two", "2")); - ReflectUtils.createJson(list); - }); - - assertTrue(exception.getMessage().startsWith("object getter value")); - } - - @Test - void createJsonByStringMapCollection() { - String expectedString = "{\"one\":\"1\",\"two\":\"2\"}"; - Map map = new HashMap<>(); - map.put("one", "1"); - map.put("two", "2"); - - String result = ReflectUtils.createJson(map); - System.out.println("result: " + result); - assertEquals(expectedString, result); - - } - - @Test - void createJsonByNumberMapCollection() { - String expectedString = "{\"one\":1,\"two\":2}"; - Map map = new HashMap<>(); - map.put("one", 1); - map.put("two", 2); - - String result = ReflectUtils.createJson(map); - System.out.println("result: " + result); - assertEquals(expectedString, result); - } - - - @Test - void createJsonByObjectMapCollection(){ - - Throwable exception = assertThrows(RoboReflectException.class, () -> { - Map map = new HashMap<>(); - map.put("one", new PathAttributeDTO("one1", "1")); - map.put("two", new PathAttributeDTO("two2", "2")); - ReflectUtils.createJson(map); - }); - - assertTrue(exception.getMessage().startsWith("object getter value")); - } + + @Test + void createJsonByObjectMapCollection() { + + Throwable exception = assertThrows(RoboReflectException.class, () -> { + Map map = new HashMap<>(); + map.put("one", new PathAttributeDTO("one1", "1")); + map.put("two", new PathAttributeDTO("two2", "2")); + ReflectUtils.createJson(map); + }); + + assertTrue(exception.getMessage().startsWith("object getter value")); + } + + private static void printInfo(String result) { + LOGGER.info("result: {}", result); + } } From 094d15de95406252239fc57627a727bc9cbf67d5 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Tue, 8 Oct 2024 23:33:20 +0200 Subject: [PATCH 08/13] [69] logger add --- .../java/com/robo4j/hw/lego/ILegoSensor.java | 4 +- .../robo4j/hw/lego/wrapper/SensorWrapper.java | 4 +- robo4j-units-lego/pom.xml | 9 ++ .../lego/GripperSonicTankPlatformExample.java | 16 ++- .../lego/InfraPushTankPlatformExample.java | 23 ++-- .../robo4j/units/lego/InfraSensorExample.java | 60 ++++----- .../lego/controller/ReportController.java | 5 +- .../hw/lego/wrapper/MotorTestWrapper.java | 16 ++- .../hw/lego/wrapper/SensorTestWrapper.java | 65 ++++------ .../robo4j/units/lego/BrickButtonTests.java | 12 +- .../robo4j/units/lego/SimpleTankTestUnit.java | 6 +- .../robo4j/units/lego/SimpleTankUnitMock.java | 5 +- robo4j-units-rpi-http/pom.xml | 9 ++ ...raDecoratedImageProducerConsumerTests.java | 84 ++++++------ .../camera/CameraImageConsumerTestUnit.java | 98 +++++++------- .../CameraImageProducerConsumerTests.java | 87 ++++++------- robo4j-units-rpi/pom.xml | 15 ++- .../accelerometer/AccelerometerExample.java | 60 ++++----- .../accelerometer/AccelerometerProcessor.java | 26 ++-- .../rpi/bno/DataEventListenerExample.java | 90 ++++++------- .../rpi/bno/VectorEventListenerExample.java | 111 ++++++++-------- .../com/robo4j/units/rpi/gps/GPSExample.java | 72 ++++++----- .../robo4j/units/rpi/gps/GPSProcessor.java | 26 ++-- .../robo4j/units/rpi/gyro/GyroExample.java | 55 ++++---- .../robo4j/units/rpi/gyro/GyroProcessor.java | 26 ++-- .../led/AdafruitAlphanumericUnitExample.java | 80 ++++++------ .../led/AdafruitBiColor24BackpackExample.java | 62 ++++----- ...afruitBiColorMatrix8x8BackpackExample.java | 65 +++++----- .../rpi/lidarlite/LaserScanProcessor.java | 29 ++--- .../rpi/lidarlite/LaserScannerExample.java | 90 ++++++------- .../robo4j/units/rpi/pad/LF710PadExample.java | 40 +++--- .../units/rpi/pwm/CalibrationUtility.java | 107 ++++++++-------- .../units/rpi/pwm/ServoUnitExample.java | 121 +++++++++--------- .../rpi/roboclaw/RoboClawUnitExample.java | 108 ++++++++-------- .../robo4j/units/rpi/camera/RaspividUnit.java | 16 +-- .../src/main/java/module-info.java | 1 + 36 files changed, 881 insertions(+), 822 deletions(-) diff --git a/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/ILegoSensor.java b/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/ILegoSensor.java index 0d6a8e55..d5fdac9d 100644 --- a/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/ILegoSensor.java +++ b/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/ILegoSensor.java @@ -29,9 +29,9 @@ */ public interface ILegoSensor { - DigitalPortEnum getPort(); + DigitalPortEnum port(); - SensorTypeEnum getType(); + SensorTypeEnum type(); String getData(); diff --git a/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/wrapper/SensorWrapper.java b/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/wrapper/SensorWrapper.java index b2b18535..4650889a 100644 --- a/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/wrapper/SensorWrapper.java +++ b/robo4j-hw-lego/src/main/java/com/robo4j/hw/lego/wrapper/SensorWrapper.java @@ -53,12 +53,12 @@ public SensorWrapper(Sensor sensor, DigitalPortEnum port, SensorTypeEnum sensorT } @Override - public SensorTypeEnum getType() { + public SensorTypeEnum type() { return sensorType; } @Override - public DigitalPortEnum getPort() { + public DigitalPortEnum port() { return port; } diff --git a/robo4j-units-lego/pom.xml b/robo4j-units-lego/pom.xml index e5a2f387..3617e881 100644 --- a/robo4j-units-lego/pom.xml +++ b/robo4j-units-lego/pom.xml @@ -44,8 +44,17 @@ com.robo4j robo4j-socket-http + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine diff --git a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/GripperSonicTankPlatformExample.java b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/GripperSonicTankPlatformExample.java index a1097971..fb20e0cf 100644 --- a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/GripperSonicTankPlatformExample.java +++ b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/GripperSonicTankPlatformExample.java @@ -21,6 +21,8 @@ import com.robo4j.RoboReference; import com.robo4j.hw.lego.util.EscapeButtonUtil; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.FileInputStream; import java.io.InputStream; @@ -28,31 +30,31 @@ /** * GripperSonicTankPlatformExample is the simple example of tank platform based LegoEV3 device * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GripperSonicTankPlatformExample { + private static final Logger LOGGER = LoggerFactory.getLogger(GripperSonicTankPlatformExample.class); public static void main(String[] args) throws Exception { - final String robo4jConfig= "robo4jGripperSonicTankPlatform.xml"; + final String robo4jConfig = "robo4jGripperSonicTankPlatform.xml"; InputStream settings = GripperSonicTankPlatformExample.class.getClassLoader().getResourceAsStream(robo4jConfig); if (args.length != 1) { - System.out.println(String.format("No file specified, using default %s", robo4jConfig)); + LOGGER.info("No file specified, using default {}", robo4jConfig); } else { settings = new FileInputStream(args[0]); } final RoboBuilder builder = new RoboBuilder(); - if(settings == null){ - System.out.println("Could not find the settings for test!"); + if (settings == null) { + LOGGER.warn("Could not find the settings for test!"); System.exit(2); } builder.add(settings); RoboContext system = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(system)); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(system)); system.start(); diff --git a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraPushTankPlatformExample.java b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraPushTankPlatformExample.java index 7613be00..9f2d7f40 100644 --- a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraPushTankPlatformExample.java +++ b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraPushTankPlatformExample.java @@ -21,9 +21,10 @@ import com.robo4j.RoboReference; import com.robo4j.hw.lego.util.EscapeButtonUtil; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.FileInputStream; -import java.io.InputStream; /** * InfraPushTankPlatformExample lego platform example with push and infra red sensor @@ -32,28 +33,28 @@ * @author Miroslav Wengner (@miragemiko) */ public class InfraPushTankPlatformExample { + private static final Logger LOGGER = LoggerFactory.getLogger(InfraPushTankPlatformExample.class); public static void main(String[] args) throws Exception { - final String robo4jSystem = "robo4jSystem.xml"; - final String robo4jConfig= "robo4jInfraPushTankPlatformExample.xml"; - final InputStream systemSystem = InfraSensorExample.class.getClassLoader().getResourceAsStream(robo4jSystem); - InputStream settings = GripperSonicTankPlatformExample.class.getClassLoader().getResourceAsStream(robo4jConfig); + final var robo4jSystem = "robo4jSystem.xml"; + final var robo4jConfig = "robo4jInfraPushTankPlatformExample.xml"; + final var systemSystem = InfraSensorExample.class.getClassLoader().getResourceAsStream(robo4jSystem); + var settings = GripperSonicTankPlatformExample.class.getClassLoader().getResourceAsStream(robo4jConfig); if (args.length != 1) { - System.out.println(String.format("No file specified, using default %s", robo4jConfig)); + LOGGER.info("No file specified, using default {}", robo4jConfig); } else { settings = new FileInputStream(args[0]); } - final RoboBuilder builder = new RoboBuilder(systemSystem); - if(settings == null){ - System.out.println("Could not find the settings for test!"); + final var builder = new RoboBuilder(systemSystem); + if (settings == null) { + LOGGER.warn("Could not find the settings for test!"); System.exit(2); } builder.add(settings); RoboContext system = builder.build(); - System.out.println(SystemUtil.printStateReport(system)); - + LOGGER.info(SystemUtil.printStateReport(system)); system.start(); diff --git a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraSensorExample.java b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraSensorExample.java index 0078bc59..caf5e104 100644 --- a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraSensorExample.java +++ b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/InfraSensorExample.java @@ -21,49 +21,51 @@ import com.robo4j.RoboReference; import com.robo4j.hw.lego.util.EscapeButtonUtil; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.FileInputStream; -import java.io.InputStream; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class InfraSensorExample { + private static final Logger LOGGER = LoggerFactory.getLogger(InfraSensorExample.class); - public static void main(String[] args) throws Exception { - final String robo4jConfig = "robo4jInfraExample.xml"; - InputStream settings = InfraSensorExample.class.getClassLoader().getResourceAsStream(robo4jConfig); - if (args.length != 1) { - System.out.println(String.format("No file specified, using default %s", robo4jConfig)); - } else { - settings = new FileInputStream(args[0]); - } + public static void main(String[] args) throws Exception { + final var robo4jConfig = "robo4jInfraExample.xml"; + var settings = InfraSensorExample.class.getClassLoader().getResourceAsStream(robo4jConfig); + if (args.length != 1) { + LOGGER.info("No file specified, using default {}", robo4jConfig); + } else { + settings = new FileInputStream(args[0]); + } - final RoboBuilder builder = new RoboBuilder(); - if (settings == null) { - System.out.println("Could not find the settings for test!"); - System.exit(2); - } + final var builder = new RoboBuilder(); + if (settings == null) { + LOGGER.warn("Could not find the settings for test!"); + System.exit(2); + } - builder.add(settings); - RoboContext system = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(system)); + builder.add(settings); + RoboContext system = builder.build(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(system)); - system.start(); + system.start(); - RoboReference lcd = system.getReference("lcd"); - lcd.sendMessage("Robo4J.io"); + RoboReference lcd = system.getReference("lcd"); + lcd.sendMessage("Robo4J.io"); - RoboReference infraSensor = system.getReference("infraSensor"); - infraSensor.sendMessage("start"); + RoboReference infraSensor = system.getReference("infraSensor"); + infraSensor.sendMessage("start"); - shutdown(system); - } + shutdown(system); + } - private static void shutdown(RoboContext system) { - EscapeButtonUtil.waitForPressAndRelease(); - system.shutdown(); - } + private static void shutdown(RoboContext system) { + EscapeButtonUtil.waitForPressAndRelease(); + system.shutdown(); + } } diff --git a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/controller/ReportController.java b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/controller/ReportController.java index 6236e9a0..84209b45 100644 --- a/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/controller/ReportController.java +++ b/robo4j-units-lego/src/examples/java/com/robo4j/units/lego/controller/ReportController.java @@ -18,12 +18,15 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class ReportController extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(ReportController.class); public ReportController(RoboContext context, String id) { super(Object.class, context, id); @@ -31,6 +34,6 @@ public ReportController(RoboContext context, String id) { @Override public void onMessage(Object message) { - System.out.println(getClass().getSimpleName() + "report: " + message.toString()); + LOGGER.info("report: {}", message.toString()); } } diff --git a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/MotorTestWrapper.java b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/MotorTestWrapper.java index 2f6f5d84..3d6abee1 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/MotorTestWrapper.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/MotorTestWrapper.java @@ -19,6 +19,8 @@ import com.robo4j.hw.lego.ILegoMotor; import com.robo4j.hw.lego.enums.AnalogPortEnum; import com.robo4j.hw.lego.enums.MotorTypeEnum; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Simple LegoMindstorm Mock Motor @@ -28,6 +30,8 @@ */ public class MotorTestWrapper implements ILegoMotor { + private static final Logger LOGGER = LoggerFactory.getLogger(MotorTestWrapper.class); + private final AnalogPortEnum port; private final MotorTypeEnum type; private boolean moving; @@ -52,25 +56,25 @@ public MotorTypeEnum getType() { @Override public void forward() { moving = true; - System.out.println(String.format("MotorTest.forward port:%s, type: %s, moving: %b ", port, type, moving)); + LOGGER.info("MotorTest.forward port:{}, type: {}, moving: {} ", port, type, moving); } @Override public void backward() { moving = true; - System.out.println(String.format("MotorTest.backward port:%s, type: %s, moving: %b ", port, type, moving)); + LOGGER.info("MotorTest.backward port:{}, type: {}, moving: {} ", port, type, moving); } @Override public void stop() { moving = false; - System.out.println(String.format("MotorTest.stop port:%s, type: %s, moving: %b ", port, type, moving)); + LOGGER.info("MotorTest.stop port:{}, type: {}, moving: {} ", port, type, moving); } @Override public void rotate(int val) { - System.out.println("rotate: " + val); + LOGGER.info("rotate: {}", val); } @Override @@ -80,14 +84,14 @@ public boolean isMoving() { @Override public void setSpeed(int speed) { - System.out.println("speed: " + speed); + LOGGER.info("speed: {}", speed); this.speed = speed; } @Override public void close() { moving = false; - System.out.println(String.format("MotorTest.close port:%s, type: %s, moving: %b ", port, type, moving)); + LOGGER.info("MotorTest.close port:{}, type: {}, moving: {} ", port, type, moving); } @Override diff --git a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/SensorTestWrapper.java b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/SensorTestWrapper.java index fd5b342f..f9bcefe1 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/SensorTestWrapper.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/SensorTestWrapper.java @@ -19,6 +19,8 @@ import com.robo4j.hw.lego.ILegoSensor; import com.robo4j.hw.lego.enums.DigitalPortEnum; import com.robo4j.hw.lego.enums.SensorTypeEnum; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Simple LegoMindstorm Mock Sensor @@ -26,45 +28,28 @@ * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ -public class SensorTestWrapper implements ILegoSensor { - - private final DigitalPortEnum port; - private final SensorTypeEnum type; - - public SensorTestWrapper(DigitalPortEnum port, SensorTypeEnum type) { - this.port = port; - this.type = type; - } - - @Override - public DigitalPortEnum getPort() { - return port; - } - - @Override - public SensorTypeEnum getType() { - return type; - } - - @Override - public String getData() { - System.out.println(String.format("SensorTest.getData port:%s, type: %s", port, type)); - return "data"; - } - - @Override - public void activate(boolean status) { - System.out.println(String.format("SensorTest.activate %s, port:%s, type: %s", status, port, type)); - } - - @Override - public void close() { - System.out.println(String.format("SensorTest.close port:%s, type: %s", port, type)); - } - - @Override - public String toString() { - return "SensorTestWrapper{" + "port=" + port + ", type=" + type + '}'; - } +public record SensorTestWrapper(DigitalPortEnum port, SensorTypeEnum type) implements ILegoSensor { + private static final Logger LOGGER = LoggerFactory.getLogger(SensorTestWrapper.class); + + @Override + public String getData() { + LOGGER.info("SensorTest.getData port:{}, type: {}", port, type); + return "data"; + } + + @Override + public void activate(boolean status) { + LOGGER.info("SensorTest.activate {}, port:{}, type: {}", status, port, type); + } + + @Override + public void close() { + LOGGER.info("SensorTest.close port:{}, type: {}", port, type); + } + + @Override + public String toString() { + return "SensorTestWrapper{" + "port=" + port + ", type=" + type + '}'; + } } diff --git a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BrickButtonTests.java b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BrickButtonTests.java index 78a72847..b80dc180 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BrickButtonTests.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BrickButtonTests.java @@ -18,8 +18,8 @@ import com.robo4j.units.lego.enums.PlateButtonEnum; import org.junit.jupiter.api.Test; - -import java.util.Set; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import static org.junit.jupiter.api.Assertions.assertNotNull; @@ -30,11 +30,13 @@ * @author Miro Wengner (@miragemiko) */ class BrickButtonTests { + private static final Logger LOGGER = LoggerFactory.getLogger(BrickButtonTests.class); @Test - void basicButtonPlateTest(){ - Set buttonNames = PlateButtonEnum.getButtonNames(); - System.out.println("buttonNames: " + buttonNames); + void basicButtonPlateTest() { + var buttonNames = PlateButtonEnum.getButtonNames(); + + LOGGER.info("buttonNames: {}", buttonNames); assertNotNull(buttonNames); } diff --git a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankTestUnit.java b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankTestUnit.java index 2d7cb6dc..3798735d 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankTestUnit.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankTestUnit.java @@ -23,6 +23,8 @@ import com.robo4j.hw.lego.enums.AnalogPortEnum; import com.robo4j.hw.lego.enums.MotorTypeEnum; import com.robo4j.hw.lego.wrapper.MotorTestWrapper; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Simple Tank Test Platform Unit @@ -31,10 +33,11 @@ * @author Miro Wengner (@miragemiko) */ public class SimpleTankTestUnit extends SimpleTankUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(SimpleTankTestUnit.class); public SimpleTankTestUnit(RoboContext context, String id) { super(context, id); - System.out.println(getClass().getSimpleName() + " constructor: id: " + id); + LOGGER.info(" constructor: id: {}", id); } @Override @@ -52,5 +55,4 @@ protected void onInitialization(Configuration configuration) throws Configuratio } - } diff --git a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankUnitMock.java b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankUnitMock.java index cb3f9a8f..145ad9de 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankUnitMock.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/SimpleTankUnitMock.java @@ -23,12 +23,15 @@ import com.robo4j.hw.lego.enums.AnalogPortEnum; import com.robo4j.hw.lego.enums.MotorTypeEnum; import com.robo4j.hw.lego.wrapper.MotorTestWrapper; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class SimpleTankUnitMock extends SimpleTankUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(SimpleTankUnitMock.class); public SimpleTankUnitMock(RoboContext context, String id) { super(context, id); @@ -45,7 +48,7 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void shutdown() { super.shutdown(); - System.out.println("executor is down"); + LOGGER.info("executor is down"); } @SuppressWarnings("unchecked") diff --git a/robo4j-units-rpi-http/pom.xml b/robo4j-units-rpi-http/pom.xml index b3d40fd3..f6140cf0 100644 --- a/robo4j-units-rpi-http/pom.xml +++ b/robo4j-units-rpi-http/pom.xml @@ -39,8 +39,17 @@ com.robo4j robo4j-units-rpi + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine diff --git a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraDecoratedImageProducerConsumerTests.java b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraDecoratedImageProducerConsumerTests.java index 36293516..38b8ddc4 100644 --- a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraDecoratedImageProducerConsumerTests.java +++ b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraDecoratedImageProducerConsumerTests.java @@ -17,14 +17,13 @@ package com.robo4j.units.rpi.http.camera; import com.robo4j.RoboBuilder; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.socket.http.codec.CameraMessage; import com.robo4j.socket.http.util.RoboHttpUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; -import java.io.InputStream; -import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -34,55 +33,56 @@ * @author Miro Wengner (@miragemiko) */ class CameraDecoratedImageProducerConsumerTests { - - @Test - void decoratorProducerConsumerTest() throws Exception { - - RoboBuilder builderProducer = new RoboBuilder( - Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemProducer.xml")); - InputStream clientConfigInputStream = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("robo_camera_producer_decorated_test.xml"); - builderProducer.add(clientConfigInputStream); - RoboContext producerSystem = builderProducer.build(); - - RoboBuilder builderConsumer = new RoboBuilder( - Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemConsumer.xml")); - InputStream serverConfigInputStream = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("robo_camera_consumer_decorated_test.xml"); - builderConsumer.add(serverConfigInputStream); - RoboContext consumerSystem = builderConsumer.build(); - - long startTime = System.currentTimeMillis(); - consumerSystem.start(); - producerSystem.start(); - - RoboReference imageProducer = producerSystem.getReference("imageController"); + private static final Logger LOGGER = LoggerFactory.getLogger(CameraDecoratedImageProducerConsumerTests.class); + + @Test + void decoratorProducerConsumerTest() throws Exception { + + var builderProducer = new RoboBuilder( + Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemProducer.xml")); + var clientConfigInputStream = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("robo_camera_producer_decorated_test.xml"); + builderProducer.add(clientConfigInputStream); + var producerSystem = builderProducer.build(); + + var builderConsumer = new RoboBuilder( + Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemConsumer.xml")); + var serverConfigInputStream = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("robo_camera_consumer_decorated_test.xml"); + builderConsumer.add(serverConfigInputStream); + var consumerSystem = builderConsumer.build(); + + long startTime = System.currentTimeMillis(); + consumerSystem.start(); + producerSystem.start(); + + RoboReference imageProducer = producerSystem.getReference("imageController"); RoboReference imageConsumer = consumerSystem.getReference("imageProcessor"); - CountDownLatch startLatchConsumer = imageConsumer + var startLatchConsumer = imageConsumer .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_START_LATCH).get(); startLatchConsumer.await(5, TimeUnit.MINUTES); - CountDownLatch imagesLatchProducer = imageProducer - .getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_GENERATED_IMAGES_LATCH).get(); - imagesLatchProducer.await(5, TimeUnit.MINUTES); - Integer totalImagesProducer = imageProducer.getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_TOTAL_IMAGES) - .get(); + var imagesLatchProducer = imageProducer + .getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_GENERATED_IMAGES_LATCH).get(); + imagesLatchProducer.await(5, TimeUnit.MINUTES); + Integer totalImagesProducer = imageProducer.getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_TOTAL_IMAGES) + .get(); - CountDownLatch imagesLatchConsumer = imageConsumer - .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_IMAGES_LATCH).get(); - imagesLatchConsumer.await(5, TimeUnit.MINUTES); - Integer totalImagesConsumer = imageConsumer.getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_RECEIVED_IMAGES) - .get(); - RoboHttpUtils.printMeasuredTime(getClass(), "duration", startTime); + var imagesLatchConsumer = imageConsumer + .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_IMAGES_LATCH).get(); + imagesLatchConsumer.await(5, TimeUnit.MINUTES); + Integer totalImagesConsumer = imageConsumer.getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_RECEIVED_IMAGES) + .get(); + RoboHttpUtils.printMeasuredTime(getClass(), "duration", startTime); assertEquals(totalImagesProducer, totalImagesConsumer); - producerSystem.shutdown(); - consumerSystem.shutdown(); + producerSystem.shutdown(); + consumerSystem.shutdown(); - System.out.println("Press to quit!"); + LOGGER.info("Press to quit!"); - } + } } diff --git a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageConsumerTestUnit.java b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageConsumerTestUnit.java index a8704861..32a8d45b 100644 --- a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageConsumerTestUnit.java +++ b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageConsumerTestUnit.java @@ -16,13 +16,11 @@ */ package com.robo4j.units.rpi.http.camera; -import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; -import com.robo4j.DefaultAttributeDescriptor; -import com.robo4j.RoboContext; -import com.robo4j.RoboUnit; +import com.robo4j.*; import com.robo4j.configuration.Configuration; import com.robo4j.socket.http.codec.CameraMessage; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Base64; import java.util.concurrent.CountDownLatch; @@ -33,34 +31,35 @@ * @author Miro Wengner (@miragemiko) */ public class CameraImageConsumerTestUnit extends RoboUnit { - public static final String ATTR_IMAGES_LATCH = "messagesLatch"; - public static final String ATTR_START_LATCH = "startLatch"; - public static final String ATTR_RECEIVED_IMAGES = "numberOfReceivedImages"; - public static final String PROP_TOTAL_NUMBER_MESSAGES = "totalNumberMessages"; + public static final String ATTR_IMAGES_LATCH = "messagesLatch"; + public static final String ATTR_START_LATCH = "startLatch"; + public static final String ATTR_RECEIVED_IMAGES = "numberOfReceivedImages"; + public static final String PROP_TOTAL_NUMBER_MESSAGES = "totalNumberMessages"; - public static final DefaultAttributeDescriptor DESCRIPTOR_IMAGES_LATCH = DefaultAttributeDescriptor - .create(CountDownLatch.class, ATTR_IMAGES_LATCH); + public static final DefaultAttributeDescriptor DESCRIPTOR_IMAGES_LATCH = DefaultAttributeDescriptor + .create(CountDownLatch.class, ATTR_IMAGES_LATCH); public static final DefaultAttributeDescriptor DESCRIPTOR_START_LATCH = DefaultAttributeDescriptor .create(CountDownLatch.class, ATTR_START_LATCH); - public static final AttributeDescriptor DESCRIPTOR_RECEIVED_IMAGES = new DefaultAttributeDescriptor<>( - Integer.class, ATTR_RECEIVED_IMAGES); + public static final AttributeDescriptor DESCRIPTOR_RECEIVED_IMAGES = new DefaultAttributeDescriptor<>( + Integer.class, ATTR_RECEIVED_IMAGES); + private static final Logger LOGGER = LoggerFactory.getLogger(CameraImageConsumerTestUnit.class); - private volatile AtomicInteger counter = new AtomicInteger(0); - private CountDownLatch startLatch = new CountDownLatch(1); + private final AtomicInteger counter = new AtomicInteger(0); + private final CountDownLatch startLatch = new CountDownLatch(1); private CountDownLatch messagesLatch; - public CameraImageConsumerTestUnit(RoboContext context, String id) { - super(CameraMessage.class, context, id); - } + public CameraImageConsumerTestUnit(RoboContext context, String id) { + super(CameraMessage.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - int totalNumber = configuration.getInteger(PROP_TOTAL_NUMBER_MESSAGES, 0); - if (totalNumber > 0) { - messagesLatch = new CountDownLatch(totalNumber); - } - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + int totalNumber = configuration.getInteger(PROP_TOTAL_NUMBER_MESSAGES, 0); + if (totalNumber > 0) { + messagesLatch = new CountDownLatch(totalNumber); + } + } @Override public void start() { @@ -69,35 +68,34 @@ public void start() { } @Override - public void onMessage(CameraMessage message) { - if (message.getImage() != null) { - final byte[] bytes = Base64.getDecoder().decode(message.getImage()); - System.out.println(getClass().getSimpleName() + " Delivered image: " + counter.incrementAndGet() + " size: " - + bytes.length + " imageSize: " + message.getImage().length()); - if (messagesLatch != null) { - messagesLatch.countDown(); - } - } else { - throw new IllegalStateException("no image view"); - } - } + public void onMessage(CameraMessage message) { + if (message.getImage() != null) { + final byte[] bytes = Base64.getDecoder().decode(message.getImage()); + LOGGER.info(" Delivered image: {} size: {} imageSize: {}", counter.incrementAndGet(), bytes.length, message.getImage().length()); + if (messagesLatch != null) { + messagesLatch.countDown(); + } + } else { + throw new IllegalStateException("no image view"); + } + } - @SuppressWarnings("unchecked") - @Override - protected synchronized R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Integer.class - && descriptor.getAttributeName().equals(ATTR_RECEIVED_IMAGES)) { - return (R) Integer.valueOf(counter.get()); - } - if (descriptor.getAttributeName().equals(ATTR_IMAGES_LATCH) - && descriptor.getAttributeType() == CountDownLatch.class) { - return (R) messagesLatch; - } + @SuppressWarnings("unchecked") + @Override + protected synchronized R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Integer.class + && descriptor.getAttributeName().equals(ATTR_RECEIVED_IMAGES)) { + return (R) Integer.valueOf(counter.get()); + } + if (descriptor.getAttributeName().equals(ATTR_IMAGES_LATCH) + && descriptor.getAttributeType() == CountDownLatch.class) { + return (R) messagesLatch; + } if (descriptor.getAttributeName().equals(ATTR_START_LATCH) && descriptor.getAttributeType() == CountDownLatch.class) { return (R) startLatch; } - return super.onGetAttribute(descriptor); - } + return super.onGetAttribute(descriptor); + } } diff --git a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerConsumerTests.java b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerConsumerTests.java index a050d946..2507975b 100644 --- a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerConsumerTests.java +++ b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerConsumerTests.java @@ -22,6 +22,8 @@ import com.robo4j.socket.http.codec.CameraMessage; import com.robo4j.socket.http.util.RoboHttpUtils; import org.junit.jupiter.api.Test; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.InputStream; import java.util.concurrent.CountDownLatch; @@ -34,59 +36,58 @@ * @author Miro Wengner (@miragemiko) */ class CameraImageProducerConsumerTests { + private static final Logger LOGGER = LoggerFactory.getLogger(CameraImageProducerConsumerTests.class); - @Test - void cameraImageProdConTest() throws Exception { + @Test + void cameraImageProdConTest() throws Exception { - RoboBuilder builderProducer = new RoboBuilder( - Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemProducer.xml")); - InputStream clientConfigInputStream = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("robo_camera_producer_test.xml"); - builderProducer.add(clientConfigInputStream); - RoboContext producerSystem = builderProducer.build(); + RoboBuilder builderProducer = new RoboBuilder( + Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemProducer.xml")); + InputStream clientConfigInputStream = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("robo_camera_producer_test.xml"); + builderProducer.add(clientConfigInputStream); + RoboContext producerSystem = builderProducer.build(); - RoboBuilder builderConsumer = new RoboBuilder( - Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemConsumer.xml")); - InputStream serverConfigInputStream = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("robo_camera_consumer_test.xml"); - builderConsumer.add(serverConfigInputStream); + RoboBuilder builderConsumer = new RoboBuilder( + Thread.currentThread().getContextClassLoader().getResourceAsStream("robo4jSystemConsumer.xml")); + InputStream serverConfigInputStream = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("robo_camera_consumer_test.xml"); + builderConsumer.add(serverConfigInputStream); - RoboContext consumerSystem = builderConsumer.build(); + RoboContext consumerSystem = builderConsumer.build(); - long startTime = System.currentTimeMillis(); - consumerSystem.start(); - producerSystem.start(); + long startTime = System.currentTimeMillis(); + consumerSystem.start(); + producerSystem.start(); RoboReference imageConsumer = consumerSystem.getReference("imageProcessor"); - RoboReference imageProducer = producerSystem.getReference("imageController"); + RoboReference imageProducer = producerSystem.getReference("imageController"); CountDownLatch startConsumerLatch = imageConsumer .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_START_LATCH).get(); startConsumerLatch.await(5, TimeUnit.MINUTES); - Integer totalImagesProducer = imageProducer.getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_TOTAL_IMAGES) - .get(); - CountDownLatch imageProducerLatch = imageProducer - .getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_GENERATED_IMAGES_LATCH).get(); - CountDownLatch imageConsumerLatch = imageConsumer - .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_IMAGES_LATCH).get(); - - System.out.println("LATCH"); - imageProducerLatch.await(5, TimeUnit.MINUTES); - System.out.println("ONE"); - imageConsumerLatch.await(5, TimeUnit.MINUTES); - System.out.println("TWO"); - - Integer receivedImagesConsumer = imageConsumer - .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_RECEIVED_IMAGES).get(); - assertEquals(totalImagesProducer, receivedImagesConsumer); - - RoboHttpUtils.printMeasuredTime(getClass(), "duration", startTime); - System.out.println("receivedImagesConsumer: " + receivedImagesConsumer); - producerSystem.shutdown(); - consumerSystem.shutdown(); - - System.out.println("Press to quit!"); - - } + Integer totalImagesProducer = imageProducer.getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_TOTAL_IMAGES) + .get(); + CountDownLatch imageProducerLatch = imageProducer + .getAttribute(CameraImageProducerDesTestUnit.DESCRIPTOR_GENERATED_IMAGES_LATCH).get(); + CountDownLatch imageConsumerLatch = imageConsumer + .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_IMAGES_LATCH).get(); + + LOGGER.info("LATCH"); + imageProducerLatch.await(5, TimeUnit.MINUTES); + LOGGER.info("ONE"); + imageConsumerLatch.await(5, TimeUnit.MINUTES); + LOGGER.info("TWO"); + + Integer receivedImagesConsumer = imageConsumer + .getAttribute(CameraImageConsumerTestUnit.DESCRIPTOR_RECEIVED_IMAGES).get(); + assertEquals(totalImagesProducer, receivedImagesConsumer); + + RoboHttpUtils.printMeasuredTime(getClass(), "duration", startTime); + LOGGER.info("receivedImagesConsumer: " + receivedImagesConsumer); + producerSystem.shutdown(); + consumerSystem.shutdown(); + + } } diff --git a/robo4j-units-rpi/pom.xml b/robo4j-units-rpi/pom.xml index f5fc5b2f..63c1b989 100644 --- a/robo4j-units-rpi/pom.xml +++ b/robo4j-units-rpi/pom.xml @@ -50,8 +50,17 @@ pi4j-plugin-linuxfs ${pi4j.version} + + org.slf4j + slf4j-api + + + org.slf4j + slf4j-simple + test + org.junit.jupiter junit-jupiter-engine @@ -96,9 +105,11 @@ - + - com.robo4j.units.rpi.led.AdafruitAlphanumericUnitExample + com.robo4j.units.rpi.led.AdafruitAlphanumericUnitExample + diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerExample.java index 634e3ad3..22b29855 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerExample.java @@ -16,50 +16,52 @@ */ package com.robo4j.units.rpi.accelerometer; -import java.io.IOException; -import java.io.InputStream; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Runs the accelerometer continuously and always prints what it reads. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AccelerometerExample { - private static final String ID_PROCESSOR = "processor"; + private static final String ID_PROCESSOR = "processor"; + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerExample.class); - public static void main(String[] args) throws RoboBuilderException, IOException { - RoboBuilder builder = new RoboBuilder(); - InputStream settings = Thread.currentThread().getContextClassLoader() - .getResourceAsStream("accelerometerexample.xml"); - if (settings == null) { - System.out.println("Could not find the settings for the GyroExample!"); - System.exit(2); - } - builder.add(settings); - builder.add(AccelerometerProcessor.class, ID_PROCESSOR); - RoboContext ctx = builder.build(); + public static void main(String[] args) throws RoboBuilderException, IOException { + var builder = new RoboBuilder(); + var settings = Thread.currentThread().getContextClassLoader() + .getResourceAsStream("accelerometerexample.xml"); + if (settings == null) { + LOGGER.warn("Could not find the settings for the GyroExample!"); + System.exit(2); + } + builder.add(settings); + builder.add(AccelerometerProcessor.class, ID_PROCESSOR); + RoboContext ctx = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - ctx.start(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - RoboReference accelerometer = ctx.getReference("accelerometer"); - RoboReference processor = ctx.getReference(ID_PROCESSOR); + RoboReference accelerometer = ctx.getReference("accelerometer"); + RoboReference processor = ctx.getReference(ID_PROCESSOR); - System.out.println("Press to start!"); - System.in.read(); - accelerometer.sendMessage(new AccelerometerRequest(processor, true, (Float3D) -> true)); - System.out.println("Will report angular changes indefinitely.\nPress to quit!"); - System.in.read(); - } + LOGGER.info("Press to start!"); + System.in.read(); + accelerometer.sendMessage(new AccelerometerRequest(processor, true, (Float3D) -> true)); + LOGGER.info("Will report angular changes indefinitely.\nPress to quit!"); + System.in.read(); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerProcessor.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerProcessor.java index ab9f8340..be3c2513 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerProcessor.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/accelerometer/AccelerometerProcessor.java @@ -20,25 +20,29 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Example recipient for testing the accelerometer. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AccelerometerProcessor extends RoboUnit { - public AccelerometerProcessor(RoboContext context, String id) { - super(AccelerometerEvent.class, context, id); - } + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerProcessor.class); + + public AccelerometerProcessor(RoboContext context, String id) { + super(AccelerometerEvent.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { - } + } - @Override - public void onMessage(AccelerometerEvent result) { - System.out.println("AccelerometerEvent: " + result.toString()); - } + @Override + public void onMessage(AccelerometerEvent result) { + LOGGER.info("AccelerometerEvent: {}", result.toString()); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/DataEventListenerExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/DataEventListenerExample.java index 655a284b..80db1e68 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/DataEventListenerExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/DataEventListenerExample.java @@ -18,11 +18,12 @@ package com.robo4j.units.rpi.bno; import com.robo4j.RoboBuilder; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.hw.rpi.imu.bno.DataEvent3f; import com.robo4j.units.rpi.imu.BnoRequest; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.InputStream; import java.nio.file.Files; @@ -38,57 +39,58 @@ * @author Miroslav Wengner (@miragemiko) */ public class DataEventListenerExample { + private static final Logger LOGGER = LoggerFactory.getLogger(DataEventListenerExample.class); - public static void main(String[] args) throws Exception { - ClassLoader classLoader = Thread.currentThread().getContextClassLoader(); + public static void main(String[] args) throws Exception { + var classLoader = Thread.currentThread().getContextClassLoader(); - final InputStream systemIS; - final InputStream contextIS; + final InputStream systemIS; + final InputStream contextIS; - switch (args.length) { - case 0: - systemIS = classLoader.getResourceAsStream("bno080DataSystemEmitterExample.xml"); - contextIS = classLoader.getResourceAsStream("bno080GyroExample.xml"); - System.out.println("Default configuration used"); - break; - case 1: - systemIS = classLoader.getResourceAsStream("bno080DataSystemEmitterExample.xml"); - Path contextPath = Paths.get(args[0]); - contextIS = Files.newInputStream(contextPath); - System.out.println("Robo4j config file has been used: " + args[0]); - break; - case 2: - Path systemPath2 = Paths.get(args[0]); - Path contextPath2 = Paths.get(args[1]); - systemIS = Files.newInputStream(systemPath2); - contextIS = Files.newInputStream(contextPath2); - System.out.println(String.format("Custom configuration used system: %s, context: %s", args[0], args[1])); - break; - default: - System.out.println("Could not find the *.xml settings for the CameraClient!"); - System.out.println("java -jar camera.jar system.xml context.xml"); - System.exit(2); - throw new IllegalStateException("see configuration"); - } + switch (args.length) { + case 0: + systemIS = classLoader.getResourceAsStream("bno080DataSystemEmitterExample.xml"); + contextIS = classLoader.getResourceAsStream("bno080GyroExample.xml"); + LOGGER.info("Default configuration used"); + break; + case 1: + systemIS = classLoader.getResourceAsStream("bno080DataSystemEmitterExample.xml"); + Path contextPath = Paths.get(args[0]); + contextIS = Files.newInputStream(contextPath); + LOGGER.info("Robo4j config file has been used: {}", args[0]); + break; + case 2: + Path systemPath2 = Paths.get(args[0]); + Path contextPath2 = Paths.get(args[1]); + systemIS = Files.newInputStream(systemPath2); + contextIS = Files.newInputStream(contextPath2); + LOGGER.info("Custom configuration used system: {}, context: {}", args[0], args[1]); + break; + default: + LOGGER.warn("Could not find the *.xml settings for the CameraClient!"); + LOGGER.warn("java -jar camera.jar system.xml context.xml"); + System.exit(2); + throw new IllegalStateException("see configuration"); + } - RoboBuilder builder = new RoboBuilder(systemIS); - builder.add(contextIS); - RoboContext ctx = builder.build(); + var builder = new RoboBuilder(systemIS); + builder.add(contextIS); + var ctx = builder.build(); - ctx.start(); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - RoboReference bnoUnit = ctx.getReference("bno"); - RoboReference bnoListenerUnit = ctx.getReference("listener"); + var bnoUnit = ctx.getReference("bno"); + RoboReference bnoListenerUnit = ctx.getReference("listener"); - BnoRequest requestToRegister = new BnoRequest(bnoListenerUnit, BnoRequest.ListenerAction.REGISTER); - bnoUnit.sendMessage(requestToRegister); + BnoRequest requestToRegister = new BnoRequest(bnoListenerUnit, BnoRequest.ListenerAction.REGISTER); + bnoUnit.sendMessage(requestToRegister); - System.out.println("Press to start!"); - System.in.read(); - ctx.shutdown(); + LOGGER.info("Press to start!"); + System.in.read(); + ctx.shutdown(); - } + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/VectorEventListenerExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/VectorEventListenerExample.java index 79beaf40..661be4bc 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/VectorEventListenerExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/bno/VectorEventListenerExample.java @@ -18,13 +18,13 @@ package com.robo4j.units.rpi.bno; import com.robo4j.RoboBuilder; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.hw.rpi.imu.bno.DataEvent3f; -import com.robo4j.net.LookupService; import com.robo4j.net.LookupServiceProvider; import com.robo4j.units.rpi.imu.BnoRequest; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.io.InputStream; @@ -40,67 +40,68 @@ * @author Miroslav Wengner (@miragemiko) */ public class VectorEventListenerExample { - public static void main(String[] args) throws Exception { - final ClassLoader classLoader = Thread.currentThread().getContextClassLoader(); + private static final Logger LOGGER = LoggerFactory.getLogger(VectorEventListenerExample.class); - final InputStream systemIS; - final InputStream contextIS; + public static void main(String[] args) throws Exception { + final ClassLoader classLoader = Thread.currentThread().getContextClassLoader(); - switch (args.length) { - case 0: - systemIS = classLoader.getResourceAsStream("bno080VectorSystemEmitterExample.xml"); - contextIS = classLoader.getResourceAsStream("bno080VectorExample.xml"); - System.out.println("Default configuration used"); - break; - case 1: - systemIS = classLoader.getResourceAsStream("bno080VectorSystemEmitterExample.xml"); - Path contextPath = Paths.get(args[0]); - contextIS = Files.newInputStream(contextPath); - System.out.println("Robo4j config file has been used: " + args[0]); - break; - case 2: - Path systemPath2 = Paths.get(args[0]); - Path contextPath2 = Paths.get(args[1]); - systemIS = Files.newInputStream(systemPath2); - contextIS = Files.newInputStream(contextPath2); - System.out.println(String.format("Custom configuration used system: %s, context: %s", args[0], args[1])); - break; - default: - System.out.println("Could not find the *.xml settings for the CameraClient!"); - System.out.println("java -jar camera.jar system.xml context.xml"); - System.exit(2); - throw new IllegalStateException("see configuration"); - } + final InputStream systemIS; + final InputStream contextIS; - if (systemIS == null && contextIS == null) { - System.out.println("Could not find the settings for the BNO080 Example!"); - System.exit(2); - } - RoboBuilder builder = new RoboBuilder(systemIS); - builder.add(contextIS); - RoboContext ctx = builder.build(); + switch (args.length) { + case 0: + systemIS = classLoader.getResourceAsStream("bno080VectorSystemEmitterExample.xml"); + contextIS = classLoader.getResourceAsStream("bno080VectorExample.xml"); + LOGGER.info("Default configuration used"); + break; + case 1: + systemIS = classLoader.getResourceAsStream("bno080VectorSystemEmitterExample.xml"); + Path contextPath = Paths.get(args[0]); + contextIS = Files.newInputStream(contextPath); + LOGGER.info("Robo4j config file has been used: {}", args[0]); + break; + case 2: + Path systemPath2 = Paths.get(args[0]); + Path contextPath2 = Paths.get(args[1]); + systemIS = Files.newInputStream(systemPath2); + contextIS = Files.newInputStream(contextPath2); + LOGGER.info("Custom configuration used system: {}, context: {}", args[0], args[1]); + break; + default: + LOGGER.warn("Could not find the *.xml settings for the CameraClient!"); + LOGGER.warn("java -jar camera.jar system.xml context.xml"); + System.exit(2); + throw new IllegalStateException("see configuration"); + } - ctx.start(); + if (systemIS == null && contextIS == null) { + LOGGER.info("Could not find the settings for the BNO080 Example!"); + System.exit(2); + } + var builder = new RoboBuilder(systemIS); + builder.add(contextIS); + var ctx = builder.build(); - LookupService service = LookupServiceProvider.getDefaultLookupService(); - try { - service.start(); - } catch (IOException e) { - e.printStackTrace(); - } + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + var service = LookupServiceProvider.getDefaultLookupService(); + try { + service.start(); + } catch (IOException e) { + LOGGER.error("error starting lookup service", e); + } - RoboReference bnoUnit = ctx.getReference("bno"); - RoboReference bnoListenerUnit = ctx.getReference("listener"); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - BnoRequest requestToRegister = new BnoRequest(bnoListenerUnit, BnoRequest.ListenerAction.REGISTER); - bnoUnit.sendMessage(requestToRegister); + RoboReference bnoUnit = ctx.getReference("bno"); + RoboReference bnoListenerUnit = ctx.getReference("listener"); - System.out.println("Press to start!"); - System.in.read(); - ctx.shutdown(); + BnoRequest requestToRegister = new BnoRequest(bnoListenerUnit, BnoRequest.ListenerAction.REGISTER); + bnoUnit.sendMessage(requestToRegister); - } + LOGGER.info("Press to start!"); + System.in.read(); + ctx.shutdown(); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSExample.java index 52834a32..458a4b68 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSExample.java @@ -16,61 +16,63 @@ */ package com.robo4j.units.rpi.gps; -import java.io.IOException; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.hw.rpi.gps.GPSEvent; import com.robo4j.units.rpi.gps.GPSRequest.Operation; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Runs the gyro continuously. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GPSExample { - private static final String ID_PROCESSOR = "processor"; - private static final String ID_GPS = "gps"; + private static final Logger LOGGER = LoggerFactory.getLogger(GPSExample.class); + private static final String ID_PROCESSOR = "processor"; + private static final String ID_GPS = "gps"; - public static void main(String[] args) throws RoboBuilderException, IOException { - RoboBuilder builder = new RoboBuilder(); - builder.add(MtkGPSUnit.class, ID_GPS); - builder.add(GPSProcessor.class, ID_PROCESSOR); - RoboContext ctx = builder.build(); + public static void main(String[] args) throws RoboBuilderException, IOException { + var builder = new RoboBuilder(); + builder.add(MtkGPSUnit.class, ID_GPS); + builder.add(GPSProcessor.class, ID_PROCESSOR); + var ctx = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - ctx.start(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - RoboReference gps = ctx.getReference(ID_GPS); - RoboReference processor = ctx.getReference(ID_PROCESSOR); + RoboReference gps = ctx.getReference(ID_GPS); + RoboReference processor = ctx.getReference(ID_PROCESSOR); - System.out.println("Press to start requesting events, then press enter again to stop requesting events!"); - System.in.read(); + LOGGER.info("Press to start requesting events, then press enter again to stop requesting events!"); + System.in.read(); - System.out.println("Requesting GPS events! Press to stop!"); - gps.sendMessage(new GPSRequest(processor, Operation.REGISTER)); - System.in.read(); + LOGGER.info("Requesting GPS events! Press to stop!"); + gps.sendMessage(new GPSRequest(processor, Operation.REGISTER)); + System.in.read(); - System.out.println("Ending requesting GPS events..."); - gps.sendMessage(new GPSRequest(processor, Operation.UNREGISTER)); - // Note that we can still get a few more events after this, and that is - // quite fine. ;) - System.out.println("All done! Press to quit!"); - System.in.read(); + LOGGER.info("Ending requesting GPS events..."); + gps.sendMessage(new GPSRequest(processor, Operation.UNREGISTER)); + // Note that we can still get a few more events after this, and that is + // quite fine. ;) + LOGGER.info("All done! Press to quit!"); + System.in.read(); - System.out.println("Exiting! Bye!"); - ctx.shutdown(); + LOGGER.info("Exiting! Bye!"); + ctx.shutdown(); - // Seems Pi4J keeps an executor with non-daemon threads around after - // we've used the serial port, even after closing it. :/ - System.exit(0); - } + // Seems Pi4J keeps an executor with non-daemon threads around after + // we've used the serial port, even after closing it. :/ + System.exit(0); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSProcessor.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSProcessor.java index 0bc520fa..f812761f 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSProcessor.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gps/GPSProcessor.java @@ -21,25 +21,29 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.gps.AbstractGPSEvent; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Example recipient for GPS events. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GPSProcessor extends RoboUnit { - public GPSProcessor(RoboContext context, String id) { - super(AbstractGPSEvent.class, context, id); - } + private static final Logger LOGGER = LoggerFactory.getLogger(GPSProcessor.class); + + public GPSProcessor(RoboContext context, String id) { + super(AbstractGPSEvent.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { - } + } - @Override - public void onMessage(AbstractGPSEvent result) { - System.out.println("GPSEvent: " + result.toString()); - } + @Override + public void onMessage(AbstractGPSEvent result) { + LOGGER.info("GPSEvent: {}", result.toString()); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroExample.java index 183d347b..e3add64f 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroExample.java @@ -18,49 +18,50 @@ import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.units.rpi.gyro.GyroRequest.GyroAction; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.io.InputStream; /** * Runs the gyro continuously. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GyroExample { - private static final String ID_PROCESSOR = "processor"; + private static final Logger LOGGER = LoggerFactory.getLogger(GyroExample.class); + private static final String ID_PROCESSOR = "processor"; - public static void main(String[] args) throws RoboBuilderException, IOException { - RoboBuilder builder = new RoboBuilder(); - InputStream settings = GyroExample.class.getClassLoader().getResourceAsStream("gyroexample.xml"); - if (settings == null) { - System.out.println("Could not find the settings for the GyroExample!"); - System.exit(2); - } - builder.add(settings); - builder.add(GyroProcessor.class, ID_PROCESSOR); - RoboContext ctx = builder.build(); + public static void main(String[] args) throws RoboBuilderException, IOException { + var builder = new RoboBuilder(); + var settings = GyroExample.class.getClassLoader().getResourceAsStream("gyroexample.xml"); + if (settings == null) { + LOGGER.warn("Could not find the settings for the GyroExample!"); + System.exit(2); + } + builder.add(settings); + builder.add(GyroProcessor.class, ID_PROCESSOR); + var ctx = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - ctx.start(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - RoboReference gyro = ctx.getReference("gyro"); - RoboReference processor = ctx.getReference(ID_PROCESSOR); + RoboReference gyro = ctx.getReference("gyro"); + RoboReference processor = ctx.getReference(ID_PROCESSOR); - System.out.println("Let the gyro unit be absolutely still, then press enter to calibrate and start!"); - System.in.read(); - gyro.sendMessage(new GyroRequest(processor, GyroAction.CONTINUOUS, new Tuple3f(1.0f, 1.0f, 1.0f))); - System.out.println("Will report angular changes indefinitely.\nPress to quit!"); - System.in.read(); - } + LOGGER.info("Let the gyro unit be absolutely still, then press enter to calibrate and start!"); + System.in.read(); + gyro.sendMessage(new GyroRequest(processor, GyroAction.CONTINUOUS, new Tuple3f(1.0f, 1.0f, 1.0f))); + LOGGER.info("Will report angular changes indefinitely.\nPress to quit!"); + System.in.read(); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroProcessor.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroProcessor.java index 40431d51..efb3bcbf 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroProcessor.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/gyro/GyroProcessor.java @@ -20,25 +20,29 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Example recipient for gyro events. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class GyroProcessor extends RoboUnit { - public GyroProcessor(RoboContext context, String id) { - super(GyroEvent.class, context, id); - } + private static final Logger LOGGER = LoggerFactory.getLogger(GyroProcessor.class); + + public GyroProcessor(RoboContext context, String id) { + super(GyroEvent.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { - } + } - @Override - public void onMessage(GyroEvent result) { - System.out.println("GyroEvent: " + result.toString()); - } + @Override + public void onMessage(GyroEvent result) { + LOGGER.info("GyroEvent: {}", result.toString()); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnitExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnitExample.java index 43fa813c..8346df01 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnitExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnitExample.java @@ -17,69 +17,67 @@ package com.robo4j.units.rpi.led; -import java.io.InputStream; -import java.nio.charset.Charset; +import com.robo4j.RoboBuilder; +import com.robo4j.RoboReference; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.nio.charset.StandardCharsets; import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicInteger; -import com.robo4j.RoboBuilder; -import com.robo4j.RoboContext; -import com.robo4j.RoboReference; - /** * https://learn.adafruit.com/adafruit-led-backpack/0-54-alphanumeric - * + *

* demo: Continually sending defined String * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitAlphanumericUnitExample { + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitAlphanumericUnitExample.class); + private static final byte[] MESSAGE = "HelloMAMAPAPAMAXIELLA".getBytes(StandardCharsets.US_ASCII); + private static final byte[] BUFFER = {' ', ' ', ' ', ' '}; - private static final byte[] MESSAGE = "HelloMAMAPAPAMAXIELLA".getBytes(StandardCharsets.US_ASCII); - private static final byte[] BUFFER = { ' ', ' ', ' ', ' ' }; - - public static void main(String[] args) throws Exception { - ScheduledExecutorService executor = Executors.newSingleThreadScheduledExecutor(); - InputStream settings = AdafruitBiColor24BackpackExample.class.getClassLoader().getResourceAsStream("alphanumericexample.xml"); - RoboContext ctx = new RoboBuilder().add(settings).build(); + public static void main(String[] args) throws Exception { + var executor = Executors.newSingleThreadScheduledExecutor(); + var settings = AdafruitBiColor24BackpackExample.class.getClassLoader().getResourceAsStream("alphanumericexample.xml"); + var ctx = new RoboBuilder().add(settings).build(); - ctx.start(); - RoboReference alphaUnit = ctx.getReference("alphanumeric"); - AtomicInteger textPosition = new AtomicInteger(); + ctx.start(); + RoboReference alphaUnit = ctx.getReference("alphanumeric"); + AtomicInteger textPosition = new AtomicInteger(); - executor.scheduleAtFixedRate(() -> { + executor.scheduleAtFixedRate(() -> { - if (textPosition.getAndIncrement() >= MESSAGE.length - 1) { - textPosition.set(0); - } + if (textPosition.getAndIncrement() >= MESSAGE.length - 1) { + textPosition.set(0); + } - alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_CLEAR); + alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_CLEAR); - byte currentChar = MESSAGE[textPosition.get()]; - adjustBuffer(currentChar); - alphaUnit.sendMessage(new AlphaNumericMessage(BackpackMessageCommand.PAINT, BUFFER.clone(), new boolean[4], 0)); - alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_DISPLAY); - }, 1, 500, TimeUnit.MILLISECONDS); + byte currentChar = MESSAGE[textPosition.get()]; + adjustBuffer(currentChar); + alphaUnit.sendMessage(new AlphaNumericMessage(BackpackMessageCommand.PAINT, BUFFER.clone(), new boolean[4], 0)); + alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_DISPLAY); + }, 1, 500, TimeUnit.MILLISECONDS); - System.out.println("Press enter to quit\n"); - System.in.read(); - alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_CLEAR); - alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_DISPLAY); - executor.shutdown(); - ctx.shutdown(); + LOGGER.info("Press enter to quit"); + System.in.read(); + alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_CLEAR); + alphaUnit.sendMessage(AlphaNumericMessage.MESSAGE_DISPLAY); + executor.shutdown(); + ctx.shutdown(); - } + } - private static void adjustBuffer(byte currentChar) { + private static void adjustBuffer(byte currentChar) { - BUFFER[3] = BUFFER[2]; - BUFFER[2] = BUFFER[1]; - BUFFER[1] = BUFFER[0]; - BUFFER[0] = currentChar; + BUFFER[3] = BUFFER[2]; + BUFFER[2] = BUFFER[1]; + BUFFER[1] = BUFFER[0]; + BUFFER[0] = currentChar; - } + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColor24BackpackExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColor24BackpackExample.java index bf927e8f..d6dbe428 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColor24BackpackExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColor24BackpackExample.java @@ -17,54 +17,56 @@ package com.robo4j.units.rpi.led; -import java.io.InputStream; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicInteger; - import com.robo4j.RoboBuilder; import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.hw.rpi.i2c.adafruitbackpack.BiColor; import com.robo4j.hw.rpi.i2c.adafruitbackpack.BiColor24BarDevice; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.InputStream; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; /** * Adafruit Bi-Color 24 Bargraph example - * + *

* demo: Incrementally turning on a led light over 24 available diodes. The each * time with different Color {@link BiColor}. The color is changing circularly. - * + *

* https://learn.adafruit.com/adafruit-led-backpack/bi-color-24-bargraph * - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitBiColor24BackpackExample { + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitBiColor24BackpackExample.class); - public static void main(String[] args) throws Exception { - ScheduledExecutorService executor = Executors.newSingleThreadScheduledExecutor(); - InputStream settings = AdafruitBiColor24BackpackExample.class.getClassLoader().getResourceAsStream("bargraph24example.xml"); - RoboContext ctx = new RoboBuilder().add(settings).build(); + public static void main(String[] args) throws Exception { + ScheduledExecutorService executor = Executors.newSingleThreadScheduledExecutor(); + InputStream settings = AdafruitBiColor24BackpackExample.class.getClassLoader().getResourceAsStream("bargraph24example.xml"); + RoboContext ctx = new RoboBuilder().add(settings).build(); - ctx.start(); - RoboReference barUnit = ctx.getReference("bargraph"); - DrawMessage clearMessage = new DrawMessage(BackpackMessageCommand.CLEAR); - AtomicInteger position = new AtomicInteger(); - executor.scheduleAtFixedRate(() -> { - if (position.get() > BiColor24BarDevice.MAX_BARS - 1) { - position.set(0); - } - barUnit.sendMessage(clearMessage); - barUnit.sendMessage(new DrawMessage(BackpackMessageCommand.PAINT, new short[] { (short) position.getAndIncrement() }, - new short[] { 0 }, new BiColor[] { BiColor.getByValue(position.get() % 3 + 1) })); - }, 2, 1, TimeUnit.SECONDS); + ctx.start(); + RoboReference barUnit = ctx.getReference("bargraph"); + DrawMessage clearMessage = new DrawMessage(BackpackMessageCommand.CLEAR); + AtomicInteger position = new AtomicInteger(); + executor.scheduleAtFixedRate(() -> { + if (position.get() > BiColor24BarDevice.MAX_BARS - 1) { + position.set(0); + } + barUnit.sendMessage(clearMessage); + barUnit.sendMessage(new DrawMessage(BackpackMessageCommand.PAINT, new short[]{(short) position.getAndIncrement()}, + new short[]{0}, new BiColor[]{BiColor.getByValue(position.get() % 3 + 1)})); + }, 2, 1, TimeUnit.SECONDS); - System.out.println("Press enter to quit\n"); - System.in.read(); - executor.shutdown(); - ctx.shutdown(); + LOGGER.info("Press enter to quit"); + System.in.read(); + executor.shutdown(); + ctx.shutdown(); - } + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColorMatrix8x8BackpackExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColorMatrix8x8BackpackExample.java index c07b29e9..d2d82e62 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColorMatrix8x8BackpackExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/led/AdafruitBiColorMatrix8x8BackpackExample.java @@ -17,54 +17,55 @@ package com.robo4j.units.rpi.led; -import java.io.InputStream; -import java.util.concurrent.Executors; -import java.util.concurrent.ScheduledExecutorService; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicInteger; - import com.robo4j.RoboBuilder; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.hw.rpi.i2c.adafruitbackpack.BiColor; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.concurrent.Executors; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; /** * Adafruit Bi-Color 8x8 Matrix example - * + *

* demo: Incrementally turning on a led light over the matrix diagonal. The each * time with different Color {@link BiColor}. The color is changing circularly. - * + *

* https://learn.adafruit.com/adafruit-led-backpack/bi-color-8x8-matrix - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitBiColorMatrix8x8BackpackExample { - public static void main(String[] args) throws Exception { + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitBiColorMatrix8x8BackpackExample.class); + + public static void main(String[] args) throws Exception { - ScheduledExecutorService executor = Executors.newSingleThreadScheduledExecutor(); - InputStream settings = AdafruitBiColorMatrix8x8BackpackExample.class.getClassLoader().getResourceAsStream("matrix8x8example.xml"); - RoboContext ctx = new RoboBuilder().add(settings).build(); + var executor = Executors.newSingleThreadScheduledExecutor(); + var settings = AdafruitBiColorMatrix8x8BackpackExample.class.getClassLoader().getResourceAsStream("matrix8x8example.xml"); + var ctx = new RoboBuilder().add(settings).build(); - ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - RoboReference matrixUnit = ctx.getReference("matrix"); - AtomicInteger position = new AtomicInteger(); - executor.scheduleAtFixedRate(() -> { - if (position.get() > 7) { - position.set(0); - } - matrixUnit.sendMessage(DrawMessage.MESSAGE_CLEAR); + ctx.start(); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + RoboReference matrixUnit = ctx.getReference("matrix"); + AtomicInteger position = new AtomicInteger(); + executor.scheduleAtFixedRate(() -> { + if (position.get() > 7) { + position.set(0); + } + matrixUnit.sendMessage(DrawMessage.MESSAGE_CLEAR); - matrixUnit.sendMessage(new DrawMessage(BackpackMessageCommand.PAINT, new short[] { (short) position.get() }, - new short[] { (short) position.getAndIncrement() }, new BiColor[] { BiColor.getByValue(position.get() % 3 + 1) })); - }, 2, 1, TimeUnit.SECONDS); + matrixUnit.sendMessage(new DrawMessage(BackpackMessageCommand.PAINT, new short[]{(short) position.get()}, + new short[]{(short) position.getAndIncrement()}, new BiColor[]{BiColor.getByValue(position.get() % 3 + 1)})); + }, 2, 1, TimeUnit.SECONDS); - System.out.println("Press enter to quit\n"); - System.in.read(); - executor.shutdown(); - ctx.shutdown(); - } + LOGGER.info("Press enter to quit\n"); + System.in.read(); + executor.shutdown(); + ctx.shutdown(); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScanProcessor.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScanProcessor.java index 79220b2c..69ff884e 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScanProcessor.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScanProcessor.java @@ -16,8 +16,6 @@ */ package com.robo4j.units.rpi.lidarlite; -import java.util.concurrent.TimeUnit; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; @@ -25,26 +23,27 @@ import com.robo4j.configuration.Configuration; import com.robo4j.math.geometry.ScanResult2D; +import java.util.concurrent.TimeUnit; + /** * Example controller for testing the laser scanner. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class LaserScanProcessor extends RoboUnit { - public LaserScanProcessor(RoboContext context, String id) { - super(ScanResult2D.class, context, id); - } + public LaserScanProcessor(RoboContext context, String id) { + super(ScanResult2D.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { - } + } - @Override - public void onMessage(ScanResult2D result) { - // System.out.println(result.toString()); - RoboReference controller = getContext().getReference("controller"); - getContext().getScheduler().schedule(controller, "scan", 5, 100, TimeUnit.SECONDS, 1); - } + @Override + public void onMessage(ScanResult2D result) { + RoboReference controller = getContext().getReference("controller"); + getContext().getScheduler().schedule(controller, "scan", 5, 100, TimeUnit.SECONDS, 1); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScannerExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScannerExample.java index 794adf5d..30282079 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScannerExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/lidarlite/LaserScannerExample.java @@ -16,67 +16,69 @@ */ package com.robo4j.units.rpi.lidarlite; -import java.io.IOException; -import java.io.InputStream; -import java.nio.file.Files; -import java.nio.file.Paths; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; import com.robo4j.units.rpi.pwm.ServoUnitExample; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.io.InputStream; +import java.nio.file.Files; +import java.nio.file.Paths; /** * Runs the laser scanner, printing the max range and min range found on stdout. * (To see all data, run with JFR and dump a recording.) - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class LaserScannerExample { + private static final Logger LOGGER = LoggerFactory.getLogger(LaserScannerExample.class); - public static void main(String[] args) throws RoboBuilderException, IOException { - float startAngle = -45.0f; - float range = 90.0f; - float step = 1.0f; - InputStream settings; + public static void main(String[] args) throws RoboBuilderException, IOException { + float startAngle = -45.0f; + float range = 90.0f; + float step = 1.0f; + InputStream settings; - switch (args.length) { - case 1: - settings = Files.newInputStream(Paths.get(args[0])); - break; - case 3: - startAngle = Float.parseFloat(args[0]); - range = Float.parseFloat(args[1]); - step = Float.parseFloat(args[2]); - default: - settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("lidarexample.xml"); - } + switch (args.length) { + case 1: + settings = Files.newInputStream(Paths.get(args[0])); + break; + case 3: + startAngle = Float.parseFloat(args[0]); + range = Float.parseFloat(args[1]); + step = Float.parseFloat(args[2]); + default: + settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("lidarexample.xml"); + } - Configuration controllerConfiguration = new ConfigurationBuilder() - .addFloat(LaserScannerTestController.CONFIG_KEY_START_ANGLE, startAngle) - .addFloat(LaserScannerTestController.CONFIG_KEY_RANGE, range).addFloat(LaserScannerTestController.CONFIG_KEY_STEP, step) - .build(); + Configuration controllerConfiguration = new ConfigurationBuilder() + .addFloat(LaserScannerTestController.CONFIG_KEY_START_ANGLE, startAngle) + .addFloat(LaserScannerTestController.CONFIG_KEY_RANGE, range).addFloat(LaserScannerTestController.CONFIG_KEY_STEP, step) + .build(); - System.out.println(String.format("Running scans with startAngle=%2.1f, range=%2.1f and step=%2.1f", startAngle, range, step)); + LOGGER.info("Running scans with startAngle={}, range={} and step={}", startAngle, range, step); - RoboBuilder builder = new RoboBuilder(); - if (settings == null) { - System.out.println("Could not find the settings for the LaserScannerExample!"); - System.exit(2); - } - builder.add(settings).add(LaserScannerTestController.class, controllerConfiguration, "controller").add(LaserScanProcessor.class, - "processor"); - RoboContext ctx = builder.build(); - RoboReference tiltServo = ctx.getReference("laserscanner.tilt"); - tiltServo.sendMessage(Float.valueOf(0)); - RoboReference reference = ctx.getReference("controller"); - ctx.start(); - System.out.println("Starting scanning for ever\nPress to quit"); - reference.sendMessage("scan"); - System.in.read(); - } + var builder = new RoboBuilder(); + if (settings == null) { + LOGGER.warn("Could not find the settings for the LaserScannerExample!"); + System.exit(2); + } + builder.add(settings).add(LaserScannerTestController.class, controllerConfiguration, "controller").add(LaserScanProcessor.class, + "processor"); + var ctx = builder.build(); + RoboReference tiltServo = ctx.getReference("laserscanner.tilt"); + tiltServo.sendMessage(Float.valueOf(0)); + RoboReference reference = ctx.getReference("controller"); + ctx.start(); + LOGGER.info("Starting scanning for ever\nPress to quit"); + reference.sendMessage("scan"); + System.in.read(); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pad/LF710PadExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pad/LF710PadExample.java index 33c03f08..811741cc 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pad/LF710PadExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pad/LF710PadExample.java @@ -18,11 +18,11 @@ import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; -import java.io.InputStream; /** * Logitech F710 Pad Example @@ -31,25 +31,25 @@ * @author Miro Wengner (@miragemiko) */ public class LF710PadExample { + private static final Logger LOGGER = LoggerFactory.getLogger(LF710PadExample.class); - public static void main(String[] args) throws RoboBuilderException, IOException { - InputStream settings = Thread.currentThread().getContextClassLoader().getResourceAsStream("logitechF710.xml"); - if (settings == null) { - System.out.println("Could not find the settings for the Gamepad!"); - System.exit(2); - } - RoboBuilder builder = new RoboBuilder(); - builder.add(settings); - RoboContext sytem = builder.build(); + public static void main(String[] args) throws RoboBuilderException, IOException { + var settings = Thread.currentThread().getContextClassLoader().getResourceAsStream("logitechF710.xml"); + if (settings == null) { + LOGGER.warn("Could not find the settings for the Gamepad!"); + System.exit(2); + } + var builder = new RoboBuilder(); + builder.add(settings); + var system = builder.build(); - System.out.println("... Gamepad buttons Example ..."); - sytem.start(); + LOGGER.info("... Gamepad buttons Example ..."); + system.start(); + LOGGER.info(SystemUtil.printStateReport(system)); - System.out.println(SystemUtil.printStateReport(sytem)); - - System.out.println("Press to quit!"); - System.in.read(); - sytem.shutdown(); - System.out.println("Bye!"); - } + LOGGER.info("Press to quit!"); + System.in.read(); + system.shutdown(); + LOGGER.info("Bye!"); + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/CalibrationUtility.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/CalibrationUtility.java index 61daf5d6..36cf43e6 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/CalibrationUtility.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/CalibrationUtility.java @@ -16,72 +16,73 @@ */ package com.robo4j.units.rpi.pwm; -import java.io.FileInputStream; -import java.io.FileNotFoundException; -import java.io.InputStream; -import java.util.Scanner; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.FileInputStream; +import java.io.FileNotFoundException; +import java.util.Scanner; /** * Small calibration utility to help fine tune a servo. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class CalibrationUtility { - public static void main(String[] args) throws RoboBuilderException, FileNotFoundException { - InputStream settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("calibration.xml"); - if (args.length != 1) { - System.out.println("No file specified, using default calibration.xml"); - } else { - settings = new FileInputStream(args[0]); - } + private static final Logger LOGGER = LoggerFactory.getLogger(CalibrationUtility.class); + + public static void main(String[] args) throws RoboBuilderException, FileNotFoundException { + var settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("calibration.xml"); + if (args.length != 1) { + LOGGER.warn("No file specified, using default calibration.xml"); + } else { + settings = new FileInputStream(args[0]); + } - RoboBuilder builder = new RoboBuilder(); - if (settings == null) { - System.out.println("Could not find the settings for servo calibration test!"); - System.exit(2); - } - builder.add(settings); - RoboContext ctx = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - ctx.start(); + var builder = new RoboBuilder(); + if (settings == null) { + LOGGER.warn("Could not find the settings for servo calibration test!"); + System.exit(2); + } + builder.add(settings); + RoboContext ctx = builder.build(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - String lastCommand; - Scanner scanner = new Scanner(System.in); - System.out.println( - "Type the servo to control and how much to move the servo, between -1 and 1. For example:\npan -1.0\nType q and enter to quit!\n"); - while (!"q".equals(lastCommand = scanner.nextLine())) { - lastCommand = lastCommand.trim(); - String[] split = lastCommand.split(" "); - if (split.length != 2) { - System.out.println("Could not parse " + lastCommand + ". Please try again!"); - continue; - } - RoboReference servoRef = ctx.getReference(split[0]); - if (servoRef == null) { - System.out.println("Could not find any robo unit named " + split[0] + ". Please try again!"); - continue; - } - try { - float value = Float.parseFloat(split[1]); - servoRef.sendMessage(value); - } catch (Exception e) { - System.out.println( - "Could not parse " + split[1] + " as a float number. Error message was: " + e.getMessage() + ". Please try again!"); - continue; - } - } - ctx.shutdown(); - scanner.close(); - } + String lastCommand; + Scanner scanner = new Scanner(System.in); + LOGGER.info( + "Type the servo to control and how much to move the servo, between -1 and 1. For example:\npan -1.0\nType q and enter to quit!\n"); + while (!"q".equals(lastCommand = scanner.nextLine())) { + lastCommand = lastCommand.trim(); + String[] split = lastCommand.split(" "); + if (split.length != 2) { + LOGGER.info("Could not parse {}. Please try again!", lastCommand); + continue; + } + RoboReference servoRef = ctx.getReference(split[0]); + if (servoRef == null) { + LOGGER.info("Could not find any robo unit named {}. Please try again!", split[0]); + continue; + } + try { + float value = Float.parseFloat(split[1]); + servoRef.sendMessage(value); + } catch (Exception e) { + LOGGER.error("Could not parse {} as a float number. Error message was: {}. Please try again!", split[1], e.getMessage()); + } + } + ctx.shutdown(); + scanner.close(); + } } \ No newline at end of file diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/ServoUnitExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/ServoUnitExample.java index 7b00b75a..bf01d2b4 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/ServoUnitExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/pwm/ServoUnitExample.java @@ -16,79 +16,82 @@ */ package com.robo4j.units.rpi.pwm; -import java.io.IOException; -import java.io.InputStream; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; -import com.robo4j.RoboContext; import com.robo4j.RoboReference; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Small example panning and tilting two servos in a pan/tilt setup. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class ServoUnitExample { - private static final int PAN_STEPS = 30; - private static final int TILT_STEPS = 10; + private static final Logger LOGGER = LoggerFactory.getLogger(ServoUnitExample.class); + private static final int PAN_STEPS = 30; + private static final int TILT_STEPS = 10; + + private static volatile boolean stop = false; - private static volatile boolean stop = false; + public static void main(String[] args) throws RoboBuilderException { + var builder = new RoboBuilder(); + var settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("servoexample.xml"); + if (settings == null) { + LOGGER.warn("Could not find the settings for the ServoUnitExample!"); + System.exit(2); + } + builder.add(settings); + var ctx = builder.build(); - public static void main(String[] args) throws RoboBuilderException { - RoboBuilder builder = new RoboBuilder(); - InputStream settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("servoexample.xml"); - if (settings == null) { - System.out.println("Could not find the settings for the ServoUnitExample!"); - System.exit(2); - } - builder.add(settings); - RoboContext ctx = builder.build(); + RoboReference panRef = ctx.getReference("pan"); + RoboReference tiltRef = ctx.getReference("tilt"); - RoboReference panRef = ctx.getReference("pan"); - RoboReference tiltRef = ctx.getReference("tilt"); + Thread thread = getThread(tiltRef, panRef); + thread.start(); - Thread thread = new Thread(new Runnable() { - @Override - public void run() { - float panDirection = 1.0f; - while (!stop) { - for (int tiltStep = 0; tiltStep < TILT_STEPS; tiltStep++) { - // Just move the tilt a quarter of max positive. - float tilt = tiltStep / (TILT_STEPS * 4.0f); - tiltRef.sendMessage(tilt); - for (int panStep = 0; panStep < PAN_STEPS; panStep++) { - if (stop) { - break; - } - float pan = (panStep * 2.0f / PAN_STEPS - 1.0f) * panDirection; - panRef.sendMessage(pan); - sleep(50); - } - panDirection *= -1; - } - } - } - }); - thread.setDaemon(true); - thread.start(); + LOGGER.info("Press to quit!"); + try { + System.in.read(); + } catch (IOException e) { + LOGGER.error("error:{}", e.getMessage(), e); + } + stop = true; + ctx.shutdown(); + } - System.out.println("Press to quit!"); - try { - System.in.read(); - } catch (IOException e) { - e.printStackTrace(); - } - stop = true; - ctx.shutdown(); - } + private static Thread getThread(RoboReference tiltRef, RoboReference panRef) { + Thread thread = new Thread(() -> { + float panDirection = 1.0f; + while (!stop) { + for (int tiltStep = 0; tiltStep < TILT_STEPS; tiltStep++) { + // Just move the tilt a quarter of max positive. + float tilt = tiltStep / (TILT_STEPS * 4.0f); + tiltRef.sendMessage(tilt); + for (int panStep = 0; panStep < PAN_STEPS; panStep++) { + if (stop) { + break; + } + float pan = (panStep * 2.0f / PAN_STEPS - 1.0f) * panDirection; + panRef.sendMessage(pan); + sleep(50); + } + panDirection *= -1; + } + } + }); + thread.setDaemon(true); + return thread; + } - private static void sleep(long time) { - try { - Thread.sleep(time); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } + private static void sleep(long time) { + try { + Thread.sleep(time); + } catch (InterruptedException e) { + LOGGER.error("error:{}", e.getMessage(), e); + } + } } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java index 1fda836c..eff5d100 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java @@ -16,76 +16,76 @@ */ package com.robo4j.units.rpi.roboclaw; -import java.io.FileInputStream; -import java.io.FileNotFoundException; -import java.io.InputStream; -import java.util.Scanner; - import com.robo4j.RoboBuilder; import com.robo4j.RoboBuilderException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.units.rpi.pwm.ServoUnitExample; import com.robo4j.util.SystemUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.FileInputStream; +import java.io.FileNotFoundException; +import java.util.Scanner; /** * Small example for driving around a roboclaw controlled robot. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class RoboClawUnitExample { + private static final Logger LOGGER = LoggerFactory.getLogger(RoboClawUnitExample.class); - public static void main(String[] args) throws RoboBuilderException, FileNotFoundException { - InputStream settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("roboclawexample.xml"); - if (args.length != 1) { - System.out.println("No file specified, using default roboclawexample.xml"); - } else { - settings = new FileInputStream(args[0]); - } + public static void main(String[] args) throws RoboBuilderException, FileNotFoundException { + var settings = ServoUnitExample.class.getClassLoader().getResourceAsStream("roboclawexample.xml"); + if (args.length != 1) { + LOGGER.warn("No file specified, using default roboclawexample.xml"); + } else { + settings = new FileInputStream(args[0]); + } - RoboBuilder builder = new RoboBuilder(); - if (settings == null) { - System.out.println("Could not find the settings for test!"); - System.exit(2); - } - builder.add(settings); - RoboContext ctx = builder.build(); - System.out.println("State before start:"); - System.out.println(SystemUtil.printStateReport(ctx)); - ctx.start(); + var builder = new RoboBuilder(); + if (settings == null) { + LOGGER.warn("Could not find the settings for test!"); + System.exit(2); + } + builder.add(settings); + RoboContext ctx = builder.build(); + LOGGER.info("State before start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); + ctx.start(); - System.out.println("State after start:"); - System.out.println(SystemUtil.printStateReport(ctx)); + LOGGER.info("State after start:"); + LOGGER.info(SystemUtil.printStateReport(ctx)); - String lastCommand = ""; - Scanner scanner = new Scanner(System.in); - System.out.println( - "Type the roboclaw unit to control and the speed [-1, 1] and angular direction[-180, 180]. For example:\ntank 1 0\nType q and enter to quit!\n"); - while (!"q".equals(lastCommand = scanner.nextLine())) { - lastCommand = lastCommand.trim(); - String[] split = lastCommand.split(" "); - if (split.length != 3) { - System.out.println("Could not parse " + lastCommand + ". Please try again!"); - continue; - } - RoboReference servoRef = ctx.getReference(split[0]); - if (servoRef == null) { - System.out.println("Could not find any robo unit named " + split[0] + ". Please try again!"); - continue; - } - try { - float speed = Float.parseFloat(split[1]); - float direction = (float) Math.toRadians(Float.parseFloat(split[2])); - servoRef.sendMessage(new MotionEvent(speed, direction)); - } catch (Exception e) { - System.out.println( - "Could not parse " + split[1] + " as a float number. Error message was: " + e.getMessage() + ". Please try again!"); - continue; - } - } - ctx.shutdown(); - scanner.close(); - } + String lastCommand = ""; + Scanner scanner = new Scanner(System.in); + LOGGER.info( + "Type the roboclaw unit to control and the speed [-1, 1] and angular direction[-180, 180]. For example:\ntank 1 0\nType q and enter to quit!\n"); + while (!"q".equals(lastCommand = scanner.nextLine())) { + lastCommand = lastCommand.trim(); + String[] split = lastCommand.split(" "); + if (split.length != 3) { + System.out.println("Could not parse " + lastCommand + ". Please try again!"); + continue; + } + RoboReference servoRef = ctx.getReference(split[0]); + if (servoRef == null) { + System.out.println("Could not find any robo unit named " + split[0] + ". Please try again!"); + continue; + } + try { + float speed = Float.parseFloat(split[1]); + float direction = (float) Math.toRadians(Float.parseFloat(split[2])); + servoRef.sendMessage(new MotionEvent(speed, direction)); + } catch (Exception e) { + LOGGER.error("Could not parse {} as a float number. Error message was: {}. Please try again!", split[1], e.getMessage()); + } + } + ctx.shutdown(); + scanner.close(); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspividUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspividUnit.java index 3d52e178..6d9236c6 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspividUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspividUnit.java @@ -21,22 +21,23 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.camera.RaspiDevice; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Unit generates the video stream on desired socket - * + *

* https://www.raspberrypi.org/app/uploads/2013/07/RaspiCam-Documentation.pdf * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class RaspividUnit extends RoboUnit { - public static final String NAME = "raspividUnit"; public static final String PROPERTY_SERVER_IP = "serverIp"; public static final String PROPERTY_SERVER_PORT = "serverPort"; + private static final Logger LOGGER = LoggerFactory.getLogger(RaspividUnit.class); private final RaspiDevice device = new RaspiDevice(); private String output; private String processId; @@ -62,27 +63,26 @@ protected void onInitialization(Configuration configuration) throws Configuratio @Override public void onMessage(RaspividRequest message) { - switch (message.getType()){ + switch (message.getType()) { case CONFIG: message.put(RpiCameraProperty.OUTPUT, output); processId = String.valueOf(device.executeCommandReturnPID(message.create())); break; case START: - SimpleLoggingUtil.info(getClass(), "not necessary start message: " + message); + LOGGER.info("not necessary start message: {}", message); break; case STOP: stop(); break; default: - SimpleLoggingUtil.error(getClass(), "message: " + message); + LOGGER.error("message: {}", message); } - } @Override public void stop() { super.stop(); device.executeCommand("kill " + processId); - System.out.println("stop raspivid : " + processId); + LOGGER.info("stop raspivid : {}", processId); } } diff --git a/robo4j-units-rpi/src/main/java/module-info.java b/robo4j-units-rpi/src/main/java/module-info.java index d52c835b..98708c5c 100644 --- a/robo4j-units-rpi/src/main/java/module-info.java +++ b/robo4j-units-rpi/src/main/java/module-info.java @@ -2,6 +2,7 @@ requires jdk.jfr; requires transitive robo4j.core; requires transitive robo4j.hw.rpi; + requires org.slf4j; exports com.robo4j.units.rpi.accelerometer; exports com.robo4j.units.rpi.camera; From d34aae4283c600ec8eee3381bddc5b8007aea6bc Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Wed, 9 Oct 2024 10:00:54 +0200 Subject: [PATCH 09/13] [69] logger update --- .../units/lego/BasicSonicSensorServoUnitMock.java | 14 ++++++++------ .../units/rpi/roboclaw/RoboClawUnitExample.java | 6 +++--- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BasicSonicSensorServoUnitMock.java b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BasicSonicSensorServoUnitMock.java index fc76999f..c5cbd134 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BasicSonicSensorServoUnitMock.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/units/lego/BasicSonicSensorServoUnitMock.java @@ -17,39 +17,42 @@ package com.robo4j.units.lego; import com.robo4j.AttributeDescriptor; -import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.hw.lego.enums.DigitalPortEnum; import com.robo4j.hw.lego.enums.SensorTypeEnum; import com.robo4j.hw.lego.wrapper.SensorTestWrapper; import com.robo4j.units.lego.sonic.LegoSonicServoMessage; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ +// TODO : verify usage public class BasicSonicSensorServoUnitMock extends BasicSonicServoUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(BasicSonicSensorServoUnitMock.class); public BasicSonicSensorServoUnitMock(RoboContext context, String id) { super(context, id); } @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { + protected void onInitialization(Configuration configuration) { super.sensor = new SensorTestWrapper(DigitalPortEnum.S3, SensorTypeEnum.SONIC); - System.out.println("on initialization"); + LOGGER.info("on initialization"); } @Override public void onMessage(LegoSonicServoMessage message) { - System.out.println("onMessage : " + message); + LOGGER.info("onMessage : {}", message); } @Override public void shutdown() { super.shutdown(); - System.out.println("executor is down"); + LOGGER.info("executor is down"); } @SuppressWarnings("unchecked") @@ -62,5 +65,4 @@ public R onGetAttribute(AttributeDescriptor attribute) { } - } diff --git a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java index eff5d100..df256201 100644 --- a/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java +++ b/robo4j-units-rpi/src/examples/java/com/robo4j/units/rpi/roboclaw/RoboClawUnitExample.java @@ -60,7 +60,7 @@ public static void main(String[] args) throws RoboBuilderException, FileNotFound LOGGER.info("State after start:"); LOGGER.info(SystemUtil.printStateReport(ctx)); - String lastCommand = ""; + String lastCommand; Scanner scanner = new Scanner(System.in); LOGGER.info( "Type the roboclaw unit to control and the speed [-1, 1] and angular direction[-180, 180]. For example:\ntank 1 0\nType q and enter to quit!\n"); @@ -68,12 +68,12 @@ public static void main(String[] args) throws RoboBuilderException, FileNotFound lastCommand = lastCommand.trim(); String[] split = lastCommand.split(" "); if (split.length != 3) { - System.out.println("Could not parse " + lastCommand + ". Please try again!"); + LOGGER.warn("Could not parse {}. Please try again!", lastCommand); continue; } RoboReference servoRef = ctx.getReference(split[0]); if (servoRef == null) { - System.out.println("Could not find any robo unit named " + split[0] + ". Please try again!"); + LOGGER.warn("Could not find any robo unit named {}. Please try again!", split[0]); continue; } try { From 3cfab5fbeea06e17427c1c1dd4b0916aedfd7aac Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Wed, 9 Oct 2024 19:57:57 +0200 Subject: [PATCH 10/13] [69] partial commit --- .../channel/AcceptSelectionKeyHandler.java | 43 +++--- .../channel/ConnectSelectionKeyHandler.java | 10 +- .../InboundHttpSocketChannelHandler.java | 136 +++++++++--------- .../OutboundDatagramSocketChannelHandler.java | 94 ++++++------ 4 files changed, 144 insertions(+), 139 deletions(-) diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/AcceptSelectionKeyHandler.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/AcceptSelectionKeyHandler.java index 775c83cf..a2b2393f 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/AcceptSelectionKeyHandler.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/AcceptSelectionKeyHandler.java @@ -16,7 +16,8 @@ */ package com.robo4j.socket.http.channel; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.nio.channels.SelectionKey; import java.nio.channels.ServerSocketChannel; @@ -29,26 +30,26 @@ * @author Miro Wengner (@miragemiko) */ public class AcceptSelectionKeyHandler implements SelectionKeyHandler { + private static final Logger LOGGER = LoggerFactory.getLogger(AcceptSelectionKeyHandler.class); + private final SelectionKey key; + private final int bufferCapacity; - private final SelectionKey key; - private final int bufferCapacity; + public AcceptSelectionKeyHandler(SelectionKey key, int bufferCapacity) { + this.key = key; + this.bufferCapacity = bufferCapacity; + } - public AcceptSelectionKeyHandler(SelectionKey key, int bufferCapacity) { - this.key = key; - this.bufferCapacity = bufferCapacity; - } - - @Override - public SelectionKey handle() { - try { - ServerSocketChannel serverChannel = (ServerSocketChannel) key.channel(); - SocketChannel channel = serverChannel.accept(); - serverChannel.socket().setReceiveBufferSize(bufferCapacity); - channel.configureBlocking(false); - channel.register(key.selector(), SelectionKey.OP_READ); - } catch (Exception e) { - SimpleLoggingUtil.error(getClass(), "handle accept", e); - } - return key; - } + @Override + public SelectionKey handle() { + try { + ServerSocketChannel serverChannel = (ServerSocketChannel) key.channel(); + SocketChannel channel = serverChannel.accept(); + serverChannel.socket().setReceiveBufferSize(bufferCapacity); + channel.configureBlocking(false); + channel.register(key.selector(), SelectionKey.OP_READ); + } catch (Exception e) { + LOGGER.error("handle accept:{}", e.getMessage(), e); + } + return key; + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/ConnectSelectionKeyHandler.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/ConnectSelectionKeyHandler.java index 8ab69a81..5c7558ac 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/ConnectSelectionKeyHandler.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/ConnectSelectionKeyHandler.java @@ -16,7 +16,8 @@ */ package com.robo4j.socket.http.channel; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.nio.channels.SelectionKey; import java.nio.channels.SocketChannel; @@ -28,6 +29,7 @@ * @author Miro Wengner (@miragemiko) */ public class ConnectSelectionKeyHandler implements SelectionKeyHandler { + private static final Logger LOGGER = LoggerFactory.getLogger(ConnectSelectionKeyHandler.class); private final SelectionKey key; @@ -37,11 +39,11 @@ public ConnectSelectionKeyHandler(SelectionKey key) { @Override public SelectionKey handle() { - try{ + try { SocketChannel channel = (SocketChannel) key.channel(); channel.finishConnect(); - } catch (Exception e){ - SimpleLoggingUtil.error(getClass(), "handle connect", e); + } catch (Exception e) { + LOGGER.error("handle connect:{}", e.getMessage(), e); } return key; } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/InboundHttpSocketChannelHandler.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/InboundHttpSocketChannelHandler.java index d258fdd3..8638f17f 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/InboundHttpSocketChannelHandler.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/InboundHttpSocketChannelHandler.java @@ -17,11 +17,12 @@ package com.robo4j.socket.http.channel; import com.robo4j.RoboContext; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.request.HttpResponseProcess; import com.robo4j.socket.http.units.CodecRegistry; import com.robo4j.socket.http.units.ServerContext; import com.robo4j.socket.http.util.ChannelUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.nio.channels.SelectionKey; @@ -32,8 +33,8 @@ import java.util.concurrent.ConcurrentHashMap; import static com.robo4j.socket.http.util.ChannelUtils.handleSelectorHandler; -import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_CODEC_REGISTRY; import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_BUFFER_CAPACITY; +import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_CODEC_REGISTRY; /** * Inbound context co @@ -42,71 +43,72 @@ * @author Miro Wengner (@miragemiko) */ public class InboundHttpSocketChannelHandler implements ChannelHandler { - - private final RoboContext context; - private final ServerContext serverContext; - private final Map outBuffers = new ConcurrentHashMap<>(); - private ServerSocketChannel socketChannel; - private boolean active; - - public InboundHttpSocketChannelHandler(RoboContext context, ServerContext serverContext) { - this.context = context; - this.serverContext = serverContext; - } - - @Override - public void start() { - if (!active) { - active = true; - context.getScheduler().execute(() -> initSocketChannel(serverContext)); - } - } - - @Override - public void stop() { - try { - if (socketChannel != null && socketChannel.isOpen()) { - active = false; - socketChannel.close(); - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "server stop problem: ", e); - } - } - - private void initSocketChannel(ServerContext serverContext) { - socketChannel = ChannelUtils.initServerSocketChannel(serverContext); - final SelectionKey key = ChannelUtils.registerSelectionKey(socketChannel); - - final CodecRegistry codecRegistry = serverContext.getPropertySafe(CodecRegistry.class, PROPERTY_CODEC_REGISTRY); - final int bufferCapacity = serverContext.getPropertySafe(Integer.class, PROPERTY_BUFFER_CAPACITY); - - while (active) { - int channelReady = ChannelUtils.getReadyChannelBySelectionKey(key); - if (channelReady == 0) { - continue; - } - - Set selectedKeys = key.selector().selectedKeys(); - Iterator selectedIterator = selectedKeys.iterator(); - - while (selectedIterator.hasNext()) { - final SelectionKey selectedKey = selectedIterator.next(); - - selectedIterator.remove(); - - if (selectedKey.isAcceptable()) { - handleSelectorHandler(new AcceptSelectionKeyHandler(selectedKey, bufferCapacity)); - } else if (selectedKey.isConnectable()) { - handleSelectorHandler(new ConnectSelectionKeyHandler(selectedKey)); - } else if (selectedKey.isReadable()) { - handleSelectorHandler(new ReadSelectionKeyHandler(context, serverContext, codecRegistry, outBuffers, selectedKey)); - } else if (selectedKey.isWritable()) { - handleSelectorHandler(new WriteSelectionKeyHandler(context, serverContext, outBuffers, selectedKey)); - } - } - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(InboundHttpSocketChannelHandler.class); + + private final RoboContext context; + private final ServerContext serverContext; + private final Map outBuffers = new ConcurrentHashMap<>(); + private ServerSocketChannel socketChannel; + private boolean active; + + public InboundHttpSocketChannelHandler(RoboContext context, ServerContext serverContext) { + this.context = context; + this.serverContext = serverContext; + } + + @Override + public void start() { + if (!active) { + active = true; + context.getScheduler().execute(() -> initSocketChannel(serverContext)); + } + } + + @Override + public void stop() { + try { + if (socketChannel != null && socketChannel.isOpen()) { + active = false; + socketChannel.close(); + } + } catch (IOException e) { + LOGGER.error("server stop problem: {}", e.getMessage(), e); + } + } + + private void initSocketChannel(ServerContext serverContext) { + socketChannel = ChannelUtils.initServerSocketChannel(serverContext); + final SelectionKey key = ChannelUtils.registerSelectionKey(socketChannel); + + final CodecRegistry codecRegistry = serverContext.getPropertySafe(CodecRegistry.class, PROPERTY_CODEC_REGISTRY); + final int bufferCapacity = serverContext.getPropertySafe(Integer.class, PROPERTY_BUFFER_CAPACITY); + + while (active) { + int channelReady = ChannelUtils.getReadyChannelBySelectionKey(key); + if (channelReady == 0) { + continue; + } + + Set selectedKeys = key.selector().selectedKeys(); + Iterator selectedIterator = selectedKeys.iterator(); + + while (selectedIterator.hasNext()) { + final SelectionKey selectedKey = selectedIterator.next(); + + selectedIterator.remove(); + + if (selectedKey.isAcceptable()) { + handleSelectorHandler(new AcceptSelectionKeyHandler(selectedKey, bufferCapacity)); + } else if (selectedKey.isConnectable()) { + handleSelectorHandler(new ConnectSelectionKeyHandler(selectedKey)); + } else if (selectedKey.isReadable()) { + handleSelectorHandler(new ReadSelectionKeyHandler(context, serverContext, codecRegistry, outBuffers, selectedKey)); + } else if (selectedKey.isWritable()) { + handleSelectorHandler(new WriteSelectionKeyHandler(context, serverContext, outBuffers, selectedKey)); + } + } + } + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/OutboundDatagramSocketChannelHandler.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/OutboundDatagramSocketChannelHandler.java index 7349138f..ef797495 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/OutboundDatagramSocketChannelHandler.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/channel/OutboundDatagramSocketChannelHandler.java @@ -17,11 +17,12 @@ package com.robo4j.socket.http.channel; import com.robo4j.RoboContext; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.units.ClientContext; import com.robo4j.socket.http.units.DatagramClientUnit; import com.robo4j.socket.http.util.ChannelBufferUtils; import com.robo4j.socket.http.util.ChannelUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.net.SocketAddress; @@ -32,59 +33,58 @@ * Inbound Datagram Handler for UDP client handles sending messages and * receiving response * - * @see DatagramClientUnit - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) + * @see DatagramClientUnit */ public class OutboundDatagramSocketChannelHandler implements ChannelHandler { + private static final Logger LOGGER = LoggerFactory.getLogger(OutboundDatagramSocketChannelHandler.class); + private final RoboContext context; + private final ClientContext clientContext; + private final byte[] payload; + private DatagramChannel channel; + private volatile boolean active; - private final RoboContext context; - private final ClientContext clientContext; - private final byte[] payload; - private DatagramChannel channel; - private volatile boolean active; - - public OutboundDatagramSocketChannelHandler(RoboContext context, ClientContext clientContext, byte[] payload) { - this.context = context; - this.clientContext = clientContext; - this.payload = payload; - } + public OutboundDatagramSocketChannelHandler(RoboContext context, ClientContext clientContext, byte[] payload) { + this.context = context; + this.clientContext = clientContext; + this.payload = payload; + } - @Override - public void start() { - if (!active) { - active = true; - context.getScheduler().execute(() -> initDatagramSocket(clientContext)); - } - } + @Override + public void start() { + if (!active) { + active = true; + context.getScheduler().execute(() -> initDatagramSocket(clientContext)); + } + } - @Override - public void stop() { - try { - if (channel != null) { - active = false; - if (channel.isConnected()) - channel.close(); - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "server stop problem: ", e); - } - } + @Override + public void stop() { + try { + if (channel != null) { + active = false; + if (channel.isConnected()) + channel.close(); + } + } catch (IOException e) { + LOGGER.error("server stop problem: {}", e.getMessage(), e); + } + } - private void initDatagramSocket(ClientContext clientContext) { - channel = ChannelUtils.initDatagramChannel(DatagramConnectionType.CLIENT, clientContext); - final ByteBuffer buffer = ByteBuffer.allocateDirect(ChannelBufferUtils.INIT_BUFFER_CAPACITY); - final SocketAddress address = ChannelUtils.getSocketAddressByContext(clientContext); - // while (active.get()){ - try { - buffer.clear(); - buffer.put(payload); - buffer.flip(); - channel.send(buffer, address); - } catch (Exception e) { - SimpleLoggingUtil.error(getClass(), "datagram problem: ", e); - } + private void initDatagramSocket(ClientContext clientContext) { + channel = ChannelUtils.initDatagramChannel(DatagramConnectionType.CLIENT, clientContext); + final ByteBuffer buffer = ByteBuffer.allocateDirect(ChannelBufferUtils.INIT_BUFFER_CAPACITY); + final SocketAddress address = ChannelUtils.getSocketAddressByContext(clientContext); + // while (active.get()){ + try { + buffer.clear(); + buffer.put(payload); + buffer.flip(); + channel.send(buffer, address); + } catch (Exception e) { + LOGGER.error("datagram problem:{}", e.getMessage(), e); + } - } + } } From 2a965c0e97b00ff7945f1701fbb0a10d579a1bf9 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Wed, 9 Oct 2024 22:03:22 +0200 Subject: [PATCH 11/13] [69] update --- .../http/request/RoboRequestCallable.java | 219 +++++++------- .../http/request/RoboRequestFactory.java | 187 ++++++------ .../socket/http/units/CodecRegistry.java | 155 +++++----- .../http/units/HttpClientNetConfigUnit.java | 49 +-- .../socket/http/units/HttpClientUnit.java | 183 ++++++----- .../robo4j/socket/http/util/ChannelUtils.java | 283 +++++++++--------- .../http/test/units/RoboHttpDynamicTests.java | 21 +- 7 files changed, 552 insertions(+), 545 deletions(-) diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java index a6ea9196..3ed8fc8e 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java @@ -19,7 +19,6 @@ import com.robo4j.AttributeDescriptor; import com.robo4j.RoboContext; import com.robo4j.RoboReference; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.dto.ClassGetSetDTO; import com.robo4j.socket.http.dto.PathAttributeDTO; import com.robo4j.socket.http.dto.PathAttributeListDTO; @@ -31,6 +30,8 @@ import com.robo4j.socket.http.util.HttpPathUtils; import com.robo4j.socket.http.util.JsonUtil; import com.robo4j.socket.http.util.ReflectUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.ArrayList; import java.util.List; @@ -46,113 +47,113 @@ * @author Miro Wengner (@miragemiko) */ public class RoboRequestCallable implements Callable { - - private final RoboContext context; - private final ServerContext serverContext; - private final HttpDecoratedRequest decoratedRequest; - private final DefaultRequestFactory factory; - - public RoboRequestCallable(RoboContext context, ServerContext serverContext, HttpDecoratedRequest decoratedRequest, - DefaultRequestFactory factory) { - Objects.requireNonNull(context, "not allowed empty context"); - Objects.requireNonNull(serverContext, "not allowed empty serverContext"); - this.context = context; - this.serverContext = serverContext; - this.decoratedRequest = decoratedRequest; - this.factory = factory; - } - - @Override - @SuppressWarnings({ "unchecked", "rawtypes" }) - public HttpResponseProcess call() throws Exception { - - final HttpResponseProcessBuilder resultBuilder = HttpResponseProcessBuilder.Builder(); - final ServerPathConfig pathConfig = serverContext.getPathConfig(decoratedRequest.getPathMethod()); - - if (isValidPath(pathConfig)) { - resultBuilder.setMethod(pathConfig.getMethod()); - resultBuilder.setPath(pathConfig.getPath()); - - switch (pathConfig.getMethod()) { - case GET: - if (pathConfig.getPath().equals(UTF8_SOLIDUS)) { - resultBuilder.setCode(StatusCode.OK); - resultBuilder.setResult(factory.processGet(context)); - } else { - - resultBuilder.setTarget(pathConfig.getRoboUnit().getId()); - final Object unitDescription; - // the system needs to have one more worker thread to evaluate Future get - final HttpRequestDenominator denominator = (HttpRequestDenominator) decoratedRequest - .getDenominator(); - final Set requestAttributes = denominator.getAttributes() - .get(HttpPathUtils.ATTRIBUTES_PATH_VALUE); - if (requestAttributes == null) { - unitDescription = factory.processGet(pathConfig); - } else if (requestAttributes.isEmpty()) { - RoboReference unit = context.getReference(pathConfig.getRoboUnit().getId()); - - PathAttributeListDTO pathAttributes = new PathAttributeListDTO(); - unit.getKnownAttributes().forEach(a -> { - PathAttributeDTO attributeDescriptor = new PathAttributeDTO(); - attributeDescriptor.setName(a.getAttributeName()); - attributeDescriptor.setValue(a.getAttributeType().getCanonicalName()); - pathAttributes.addAttribute(attributeDescriptor); - }); - unitDescription = ReflectUtils.createJson(pathAttributes); - } else { - RoboReference unit = context.getReference(pathConfig.getRoboUnit().getId()); - - List attributes = new ArrayList<>(); - for (AttributeDescriptor attr : unit.getKnownAttributes()) { - if (requestAttributes.contains(attr.getAttributeName())) { - PathAttributeDTO attribute = new PathAttributeDTO(); - String valueString = String.valueOf(unit.getAttribute(attr).get()); - attribute.setValue(valueString); - attribute.setName(attr.getAttributeName()); - attributes.add(attribute); - } - } - if (attributes.size() == 1) { - Map responseAttributeDescriptorMap = ReflectUtils - .getFieldsTypeMap(PathAttributeDTO.class); - unitDescription = JsonUtil.toJson(responseAttributeDescriptorMap, attributes.get(0)); - } else { - unitDescription = JsonUtil.toJsonArray(attributes); - } - } - - resultBuilder.setCode(StatusCode.OK); - resultBuilder.setResult(unitDescription); - } - break; - case POST: - if (pathConfig.getPath().equals(UTF8_SOLIDUS)) { - resultBuilder.setCode(StatusCode.BAD_REQUEST); - } else { - resultBuilder.setTarget(pathConfig.getRoboUnit().getId()); - Object respObj = factory.processPost(pathConfig.getRoboUnit(), decoratedRequest.getMessage()); - if (respObj == null) { - resultBuilder.setCode(StatusCode.BAD_REQUEST); - } else { - resultBuilder.setCode(StatusCode.ACCEPTED); - resultBuilder.setResult(respObj); - } - } - break; - default: - resultBuilder.setCode(StatusCode.BAD_REQUEST); - SimpleLoggingUtil.debug(getClass(), "not implemented method: " + decoratedRequest.getPathMethod()); - } - } else { - resultBuilder.setCode(StatusCode.BAD_REQUEST); - } - return resultBuilder.build(); - } - - private boolean isValidPath(ServerPathConfig pathConfig) { - return pathConfig != null && decoratedRequest.getPathMethod() != null - && decoratedRequest.getPathMethod().getMethod().equals(pathConfig.getMethod()); - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboRequestCallable.class); + private final RoboContext context; + private final ServerContext serverContext; + private final HttpDecoratedRequest decoratedRequest; + private final DefaultRequestFactory factory; + + public RoboRequestCallable(RoboContext context, ServerContext serverContext, HttpDecoratedRequest decoratedRequest, + DefaultRequestFactory factory) { + Objects.requireNonNull(context, "not allowed empty context"); + Objects.requireNonNull(serverContext, "not allowed empty serverContext"); + this.context = context; + this.serverContext = serverContext; + this.decoratedRequest = decoratedRequest; + this.factory = factory; + } + + @Override + @SuppressWarnings({"unchecked", "rawtypes"}) + public HttpResponseProcess call() throws Exception { + + final HttpResponseProcessBuilder resultBuilder = HttpResponseProcessBuilder.Builder(); + final ServerPathConfig pathConfig = serverContext.getPathConfig(decoratedRequest.getPathMethod()); + + if (isValidPath(pathConfig)) { + resultBuilder.setMethod(pathConfig.getMethod()); + resultBuilder.setPath(pathConfig.getPath()); + + switch (pathConfig.getMethod()) { + case GET: + if (pathConfig.getPath().equals(UTF8_SOLIDUS)) { + resultBuilder.setCode(StatusCode.OK); + resultBuilder.setResult(factory.processGet(context)); + } else { + + resultBuilder.setTarget(pathConfig.getRoboUnit().getId()); + final Object unitDescription; + // the system needs to have one more worker thread to evaluate Future get + final HttpRequestDenominator denominator = (HttpRequestDenominator) decoratedRequest + .getDenominator(); + final Set requestAttributes = denominator.getAttributes() + .get(HttpPathUtils.ATTRIBUTES_PATH_VALUE); + if (requestAttributes == null) { + unitDescription = factory.processGet(pathConfig); + } else if (requestAttributes.isEmpty()) { + RoboReference unit = context.getReference(pathConfig.getRoboUnit().getId()); + + PathAttributeListDTO pathAttributes = new PathAttributeListDTO(); + unit.getKnownAttributes().forEach(a -> { + PathAttributeDTO attributeDescriptor = new PathAttributeDTO(); + attributeDescriptor.setName(a.getAttributeName()); + attributeDescriptor.setValue(a.getAttributeType().getCanonicalName()); + pathAttributes.addAttribute(attributeDescriptor); + }); + unitDescription = ReflectUtils.createJson(pathAttributes); + } else { + RoboReference unit = context.getReference(pathConfig.getRoboUnit().getId()); + + List attributes = new ArrayList<>(); + for (AttributeDescriptor attr : unit.getKnownAttributes()) { + if (requestAttributes.contains(attr.getAttributeName())) { + PathAttributeDTO attribute = new PathAttributeDTO(); + String valueString = String.valueOf(unit.getAttribute(attr).get()); + attribute.setValue(valueString); + attribute.setName(attr.getAttributeName()); + attributes.add(attribute); + } + } + if (attributes.size() == 1) { + Map responseAttributeDescriptorMap = ReflectUtils + .getFieldsTypeMap(PathAttributeDTO.class); + unitDescription = JsonUtil.toJson(responseAttributeDescriptorMap, attributes.get(0)); + } else { + unitDescription = JsonUtil.toJsonArray(attributes); + } + } + + resultBuilder.setCode(StatusCode.OK); + resultBuilder.setResult(unitDescription); + } + break; + case POST: + if (pathConfig.getPath().equals(UTF8_SOLIDUS)) { + resultBuilder.setCode(StatusCode.BAD_REQUEST); + } else { + resultBuilder.setTarget(pathConfig.getRoboUnit().getId()); + Object respObj = factory.processPost(pathConfig.getRoboUnit(), decoratedRequest.getMessage()); + if (respObj == null) { + resultBuilder.setCode(StatusCode.BAD_REQUEST); + } else { + resultBuilder.setCode(StatusCode.ACCEPTED); + resultBuilder.setResult(respObj); + } + } + break; + default: + resultBuilder.setCode(StatusCode.BAD_REQUEST); + LOGGER.warn("not implemented method:{}", decoratedRequest.getPathMethod()); + } + } else { + resultBuilder.setCode(StatusCode.BAD_REQUEST); + } + return resultBuilder.build(); + } + + private boolean isValidPath(ServerPathConfig pathConfig) { + return pathConfig != null && decoratedRequest.getPathMethod() != null + && decoratedRequest.getPathMethod().getMethod().equals(pathConfig.getMethod()); + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java index 47d90be4..9ee800c5 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java @@ -18,7 +18,6 @@ import com.robo4j.RoboContext; import com.robo4j.RoboReference; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.HttpMethod; import com.robo4j.socket.http.dto.ResponseAttributeDTO; import com.robo4j.socket.http.dto.ResponseDecoderUnitDTO; @@ -29,6 +28,8 @@ import com.robo4j.socket.http.units.SocketDecoder; import com.robo4j.socket.http.util.JsonUtil; import com.robo4j.socket.http.util.ReflectUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Arrays; import java.util.List; @@ -43,98 +44,96 @@ * @author Miro Wengner (@miragemiko) */ public class RoboRequestFactory implements DefaultRequestFactory { - private static final List GET_POST_METHODS = Arrays.asList(HttpMethod.GET, HttpMethod.POST); - private final CodecRegistry codecRegistry; - - public RoboRequestFactory(final CodecRegistry codecRegistry) { - this.codecRegistry = codecRegistry; - } - - /** - * Generic robo context overview. It returns all units registered into the context including system id. - * The 1st position is reserved for the system - * - * @param context - * robo context - * @return descripton of desired context - */ - @Override - public Object processGet(RoboContext context) { - if (!context.getUnits().isEmpty()) { - - final List unitList = context.getUnits().stream() - .map(u -> new ResponseUnitDTO(u.getId(), u.getState())).collect(Collectors.toList()); - unitList.add(0, new ResponseUnitDTO(context.getId(), context.getState())); - return JsonUtil.toJsonArray(unitList); - } else { - SimpleLoggingUtil.error(getClass(), "internal error: no units available"); - } - return null; - } - - // FIXME correct available methods according to the configuration - @Override - public Object processGet(ServerPathConfig pathConfig) { - final RoboReference unitRef = pathConfig.getRoboUnit(); - final SocketDecoder decoder = codecRegistry.getDecoder(unitRef.getMessageType()); - - if(unitRef.getMessageType().equals(Object.class) || decoder == null){ - List attrList = unitRef.getKnownAttributes().stream() - .map(d -> { - try { - Object val = unitRef.getAttribute(d).get(); - ResponseAttributeDTO attributeDTO = new ResponseAttributeDTO(); - attributeDTO.setId(d.getAttributeName()); - attributeDTO.setType(d.getAttributeType().getTypeName()); - attributeDTO.setValue(String.valueOf(val)); - - if(d.getAttributeName().equals(HttpServerUnit.ATTR_PATHS)){ - attributeDTO.setType("java.util.ArrayList"); - } - return attributeDTO; - - - } catch (InterruptedException | ExecutionException e) { - SimpleLoggingUtil.error(getClass(), e.getMessage()); - return null; - } - }) - .filter(Objects::nonNull) - .collect(Collectors.toList()); - return JsonUtil.toJsonArrayServer(attrList); - - } else { - final ResponseDecoderUnitDTO result = new ResponseDecoderUnitDTO(); - result.setId(unitRef.getId()); - result.setCodec(decoder.getDecodedClass().getName()); - result.setMethods(GET_POST_METHODS); - return ReflectUtils.createJson(result); - } - - - } - - @Override - public Object processServerGet(ServerPathConfig pathConfig) { - final ResponseDecoderUnitDTO result = new ResponseDecoderUnitDTO(); - return ReflectUtils.createJson(result); - } - - /** - * currently is supported POST message in JSON format - * - * example: { "value" : "move" } - * - * @param unitReference - * desired unit - * @param message - * string message - * @return processed object - */ - @Override - public Object processPost(final RoboReference unitReference, final String message) { - final SocketDecoder decoder = codecRegistry.getDecoder(unitReference.getMessageType()); - return decoder != null ? decoder.decode(message) : null; - } + private static final Logger LOGGER = LoggerFactory.getLogger(RoboRequestFactory.class); + private static final List GET_POST_METHODS = Arrays.asList(HttpMethod.GET, HttpMethod.POST); + private final CodecRegistry codecRegistry; + + public RoboRequestFactory(final CodecRegistry codecRegistry) { + this.codecRegistry = codecRegistry; + } + + /** + * Generic robo context overview. It returns all units registered into the context including system id. + * The 1st position is reserved for the system + * + * @param context robo context + * @return descripton of desired context + */ + @Override + public Object processGet(RoboContext context) { + if (!context.getUnits().isEmpty()) { + + final List unitList = context.getUnits().stream() + .map(u -> new ResponseUnitDTO(u.getId(), u.getState())).collect(Collectors.toList()); + unitList.add(0, new ResponseUnitDTO(context.getId(), context.getState())); + return JsonUtil.toJsonArray(unitList); + } else { + LOGGER.error("internal error: no units available"); + } + return null; + } + + // FIXME correct available methods according to the configuration + @Override + public Object processGet(ServerPathConfig pathConfig) { + final RoboReference unitRef = pathConfig.getRoboUnit(); + final SocketDecoder decoder = codecRegistry.getDecoder(unitRef.getMessageType()); + + if (unitRef.getMessageType().equals(Object.class) || decoder == null) { + List attrList = unitRef.getKnownAttributes().stream() + .map(d -> { + try { + Object val = unitRef.getAttribute(d).get(); + ResponseAttributeDTO attributeDTO = new ResponseAttributeDTO(); + attributeDTO.setId(d.getAttributeName()); + attributeDTO.setType(d.getAttributeType().getTypeName()); + attributeDTO.setValue(String.valueOf(val)); + + if (d.getAttributeName().equals(HttpServerUnit.ATTR_PATHS)) { + attributeDTO.setType("java.util.ArrayList"); + } + return attributeDTO; + + + } catch (InterruptedException | ExecutionException e) { + LOGGER.error("error:{}", e.getMessage(), e); + return null; + } + }) + .filter(Objects::nonNull) + .collect(Collectors.toList()); + return JsonUtil.toJsonArrayServer(attrList); + + } else { + final ResponseDecoderUnitDTO result = new ResponseDecoderUnitDTO(); + result.setId(unitRef.getId()); + result.setCodec(decoder.getDecodedClass().getName()); + result.setMethods(GET_POST_METHODS); + return ReflectUtils.createJson(result); + } + + + } + + @Override + public Object processServerGet(ServerPathConfig pathConfig) { + final ResponseDecoderUnitDTO result = new ResponseDecoderUnitDTO(); + return ReflectUtils.createJson(result); + } + + /** + * currently is supported POST message in JSON format + *

+ * example: { "value" : "move" } + * + * @param unitReference desired unit + * @param message string message + * @return processed object + */ + @Override + public Object processPost(final RoboReference unitReference, final String message) { + final SocketDecoder decoder = codecRegistry.getDecoder(unitReference.getMessageType()); + return decoder != null ? decoder.decode(message) : null; + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/CodecRegistry.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/CodecRegistry.java index 02172339..6b2544b7 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/CodecRegistry.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/CodecRegistry.java @@ -16,8 +16,9 @@ */ package com.robo4j.socket.http.units; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.reflect.ReflectionScan; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.lang.reflect.InvocationTargetException; import java.util.List; @@ -26,83 +27,85 @@ /** * Registry for codecs. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public final class CodecRegistry { - private final Map, SocketEncoder> encoders = new ConcurrentHashMap<>(); - private final Map, SocketDecoder> decoders = new ConcurrentHashMap<>(); - - public CodecRegistry() { - registerDefaults(); - } - - public CodecRegistry(String... packages) { - this(); - scan(Thread.currentThread().getContextClassLoader(), packages); - } - - public CodecRegistry(ClassLoader classLoader, String... packages){ - this(); - scan(classLoader, packages); - } - - private void scan(ClassLoader loader, String... packages) { - ReflectionScan scan = new ReflectionScan(loader); - processClasses(loader, scan.scanForEntities(packages)); - } - - public boolean containsEncoder(Class clazz){ - return encoders.containsKey(clazz); - } - - public boolean containsDecoced(Class clazz){ - return decoders.containsKey(clazz); - } - - public boolean isEmpty(){ - return encoders.isEmpty() && decoders.isEmpty(); - } - - @SuppressWarnings("unchecked") - public SocketEncoder getEncoder(Class type) { - return (SocketEncoder) encoders.get(type); - } - - @SuppressWarnings("unchecked") - public SocketDecoder getDecoder(Class type) { - return (SocketDecoder) decoders.get(type); - } - - private void registerDefaults() { - scan(CodecRegistry.class.getClassLoader(), "com.robo4j.socket.http.codec"); - } - - private void processClasses(ClassLoader loader, List allClasses) { - - for (String className : allClasses) { - try { - Class loadedClass = loader.loadClass(className); - if (loadedClass.isAnnotationPresent(HttpProducer.class)) { - addInstance(loadedClass); - } - } catch (InstantiationException | ClassNotFoundException | IllegalAccessException | NoSuchMethodException | InvocationTargetException e) { - SimpleLoggingUtil.error(getClass(), "Failed to load encoder/decoder", e); - } - } - } - - private void addInstance(Class loadedClass) throws InstantiationException, IllegalAccessException, NoSuchMethodException, InvocationTargetException { - Object instance = loadedClass.getDeclaredConstructor().newInstance(); - if (instance instanceof SocketEncoder) { - SocketEncoder encoder = (SocketEncoder) instance; - encoders.put(encoder.getEncodedClass(), encoder); - } - // Note, not "else if". People are free to implement both in the same - if (instance instanceof SocketDecoder) { - SocketDecoder decoder = (SocketDecoder) instance; - decoders.put(decoder.getDecodedClass(), decoder); - } - } + private static final Logger LOGGER = LoggerFactory.getLogger(CodecRegistry.class); + private final Map, SocketEncoder> encoders = new ConcurrentHashMap<>(); + private final Map, SocketDecoder> decoders = new ConcurrentHashMap<>(); + + public CodecRegistry() { + registerDefaults(); + } + + public CodecRegistry(String... packages) { + this(); + scan(Thread.currentThread().getContextClassLoader(), packages); + } + + public CodecRegistry(ClassLoader classLoader, String... packages) { + this(); + scan(classLoader, packages); + } + + private void scan(ClassLoader loader, String... packages) { + ReflectionScan scan = new ReflectionScan(loader); + processClasses(loader, scan.scanForEntities(packages)); + } + + public boolean containsEncoder(Class clazz) { + return encoders.containsKey(clazz); + } + + public boolean containsDecoced(Class clazz) { + return decoders.containsKey(clazz); + } + + public boolean isEmpty() { + return encoders.isEmpty() && decoders.isEmpty(); + } + + @SuppressWarnings("unchecked") + public SocketEncoder getEncoder(Class type) { + return (SocketEncoder) encoders.get(type); + } + + @SuppressWarnings("unchecked") + public SocketDecoder getDecoder(Class type) { + return (SocketDecoder) decoders.get(type); + } + + private void registerDefaults() { + scan(CodecRegistry.class.getClassLoader(), "com.robo4j.socket.http.codec"); + } + + private void processClasses(ClassLoader loader, List allClasses) { + + for (String className : allClasses) { + try { + Class loadedClass = loader.loadClass(className); + if (loadedClass.isAnnotationPresent(HttpProducer.class)) { + addInstance(loadedClass); + } + } catch (InstantiationException | ClassNotFoundException | IllegalAccessException | NoSuchMethodException | + InvocationTargetException e) { + LOGGER.error("Failed to load encoder/decoder:{}", e.getMessage(), e); + } + } + } + + private void addInstance(Class loadedClass) throws InstantiationException, IllegalAccessException, NoSuchMethodException, InvocationTargetException { + Object instance = loadedClass.getDeclaredConstructor().newInstance(); + if (instance instanceof SocketEncoder) { + SocketEncoder encoder = (SocketEncoder) instance; + encoders.put(encoder.getEncodedClass(), encoder); + } + // Note, not "else if". People are free to implement both in the same + if (instance instanceof SocketDecoder) { + SocketDecoder decoder = (SocketDecoder) instance; + decoders.put(decoder.getDecodedClass(), decoder); + } + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientNetConfigUnit.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientNetConfigUnit.java index 72f6e556..dd30d565 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientNetConfigUnit.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientNetConfigUnit.java @@ -21,9 +21,10 @@ import com.robo4j.RoboReference; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.message.HttpDecoratedRequest; import com.robo4j.socket.http.util.RoboHttpUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Map; @@ -36,35 +37,35 @@ @SuppressWarnings({"unchecked", "rawtypes"}) public class HttpClientNetConfigUnit extends RoboUnit { - public static final String PROPERTY_TARGET = "target"; + private static final Logger LOGGER = LoggerFactory.getLogger(HttpClientNetConfigUnit.class); private String target; - public HttpClientNetConfigUnit(RoboContext context, String id) { - super(Map.class, context, id); - } + public HttpClientNetConfigUnit(RoboContext context, String id) { + super(Map.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString(PROPERTY_TARGET, null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); - } - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString(PROPERTY_TARGET, null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); + } + } - @Override - public void onMessage(Map message) { - Map map = (Map) message; - SimpleLoggingUtil.info(getClass(), "RECEIVED: " + map); + @Override + public void onMessage(Map message) { + Map map = (Map) message; + LOGGER.info("RECEIVED:{}", map); - String host = map.get(RoboHttpUtils.PROPERTY_HOST); - Integer port = Integer.valueOf(map.get(RoboHttpUtils.PROPERTY_SOCKET_PORT)); + String host = map.get(RoboHttpUtils.PROPERTY_HOST); + Integer port = Integer.valueOf(map.get(RoboHttpUtils.PROPERTY_SOCKET_PORT)); - RoboReference httpUnitRef = getContext().getReference(target); - HttpDecoratedRequest confMessage = new HttpDecoratedRequest(); - confMessage.setHost(host); - confMessage.setPort(port); - httpUnitRef.sendMessage(confMessage); + RoboReference httpUnitRef = getContext().getReference(target); + HttpDecoratedRequest confMessage = new HttpDecoratedRequest(); + confMessage.setHost(host); + confMessage.setPort(port); + httpUnitRef.sendMessage(confMessage); - } + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientUnit.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientUnit.java index 3194fbb8..57ac42f9 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientUnit.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpClientUnit.java @@ -21,12 +21,13 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.ProtocolType; import com.robo4j.socket.http.channel.OutboundHttpSocketChannelHandler; import com.robo4j.socket.http.enums.StatusCode; import com.robo4j.socket.http.message.HttpDecoratedRequest; import com.robo4j.socket.http.message.HttpDecoratedResponse; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.net.InetSocketAddress; import java.nio.channels.SocketChannel; @@ -43,102 +44,100 @@ /** * Http NIO Client for communication with external Robo4J units. Unit accepts - * @see HttpDecoratedRequest type of message. Such message contains all - * necessary information and HttpClientDecorator unit is only implementation - * detail. * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) + * @see HttpDecoratedRequest type of message. Such message contains all + * necessary information and HttpClientDecorator unit is only implementation + * detail. */ @CriticalSectionTrait public class HttpClientUnit extends RoboUnit { - - private static final EnumSet PROCESS_RESPONSES_STATUSES = EnumSet.of(StatusCode.OK, - StatusCode.ACCEPTED); - private volatile String host; - private volatile Integer port; - private Integer bufferCapacity; - private ProtocolType protocol; - private Lock lock = new ReentrantLock(); - - public HttpClientUnit(RoboContext context, String id) { - super(HttpDecoratedRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - bufferCapacity = configuration.getInteger(PROPERTY_BUFFER_CAPACITY, null); - protocol = ProtocolType.valueOf(configuration.getString(HTTP_PROPERTY_PROTOCOL, "HTTP")); - host = configuration.getString(PROPERTY_HOST, null); - port = configuration.getInteger(PROPERTY_SOCKET_PORT, null); - Objects.requireNonNull(host, "host required"); - if (port == null) { - port = protocol.getPort(); - } - } - - @Override - public void onMessage(HttpDecoratedRequest message) { - final HttpDecoratedRequest request = adjustRequest(message); - if (message.getDenominator() == null) { - SimpleLoggingUtil.info(getClass(), String.format("recofigured host: %s, port: %d", message.getHost(), message.getPort())); - return; - } - final InetSocketAddress address = new InetSocketAddress(request.getHost(), request.getPort()); - try (SocketChannel channel = SocketChannel.open(address)) { - if (bufferCapacity != null) { - channel.socket().setSendBufferSize(bufferCapacity); - } - - final HttpDecoratedResponse decoratedResponse = processRequestByChannel(channel, request); - - if (decoratedResponse != null && PROCESS_RESPONSES_STATUSES.contains(decoratedResponse.getCode())) { - if (!decoratedResponse.getCallbacks().isEmpty()) { - sendMessageToCallbacks(decoratedResponse.getCallbacks(), decoratedResponse.getMessage()); - } - } else { - SimpleLoggingUtil.error(getClass(), - String.format("no callback or wrong response: %s", decoratedResponse)); - } - - } catch (Exception e) { - SimpleLoggingUtil.error(getClass(), - String.format("not available: %s, no worry I continue sending. Error: %s", address, e)); - } - } - - private HttpDecoratedResponse processRequestByChannel(SocketChannel byteChannel, - HttpDecoratedRequest message){ - try (OutboundHttpSocketChannelHandler handler = new OutboundHttpSocketChannelHandler(byteChannel, message)){ - handler.start(); - return handler.getDecoratedResponse(); - } - } - - private HttpDecoratedRequest adjustRequest(HttpDecoratedRequest request) { - lock.lock(); - try { - if(request.getHost() != null && !request.getHost().isEmpty()){ - host = request.getHost(); - } - if(request.getPort() != null){ - port = request.getPort(); - } - }finally { - lock.unlock(); - } - request.setHost(host); - request.setPort(port); - request.addHostHeader(); - return request; - } - - private void sendMessageToCallbacks(List callbacks, Object message) { - callbacks.forEach(callback -> sendMessageToCallback(callback, message)); - } - - private void sendMessageToCallback(String callback, Object message) { - getContext().getReference(callback).sendMessage(message); - } + private static final Logger LOGGER = LoggerFactory.getLogger(HttpClientUnit.class); + private static final EnumSet PROCESS_RESPONSES_STATUSES = EnumSet.of(StatusCode.OK, + StatusCode.ACCEPTED); + private volatile String host; + private volatile Integer port; + private Integer bufferCapacity; + private ProtocolType protocol; + private Lock lock = new ReentrantLock(); + + public HttpClientUnit(RoboContext context, String id) { + super(HttpDecoratedRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + bufferCapacity = configuration.getInteger(PROPERTY_BUFFER_CAPACITY, null); + protocol = ProtocolType.valueOf(configuration.getString(HTTP_PROPERTY_PROTOCOL, "HTTP")); + host = configuration.getString(PROPERTY_HOST, null); + port = configuration.getInteger(PROPERTY_SOCKET_PORT, null); + Objects.requireNonNull(host, "host required"); + if (port == null) { + port = protocol.getPort(); + } + } + + @Override + public void onMessage(HttpDecoratedRequest message) { + final HttpDecoratedRequest request = adjustRequest(message); + if (message.getDenominator() == null) { + LOGGER.info("reconfigured host: {}, port: {}", message.getHost(), message.getPort()); + return; + } + final InetSocketAddress address = new InetSocketAddress(request.getHost(), request.getPort()); + try (SocketChannel channel = SocketChannel.open(address)) { + if (bufferCapacity != null) { + channel.socket().setSendBufferSize(bufferCapacity); + } + + final HttpDecoratedResponse decoratedResponse = processRequestByChannel(channel, request); + + if (decoratedResponse != null && PROCESS_RESPONSES_STATUSES.contains(decoratedResponse.getCode())) { + if (!decoratedResponse.getCallbacks().isEmpty()) { + sendMessageToCallbacks(decoratedResponse.getCallbacks(), decoratedResponse.getMessage()); + } + } else { + LOGGER.warn("no callback or wrong response: {}", decoratedResponse); + } + + } catch (Exception e) { + LOGGER.error("not available: {}, no worry I continue sending. Error: {}", address, e.getMessage(), e); + } + } + + private HttpDecoratedResponse processRequestByChannel(SocketChannel byteChannel, + HttpDecoratedRequest message) { + try (OutboundHttpSocketChannelHandler handler = new OutboundHttpSocketChannelHandler(byteChannel, message)) { + handler.start(); + return handler.getDecoratedResponse(); + } + } + + private HttpDecoratedRequest adjustRequest(HttpDecoratedRequest request) { + lock.lock(); + try { + if (request.getHost() != null && !request.getHost().isEmpty()) { + host = request.getHost(); + } + if (request.getPort() != null) { + port = request.getPort(); + } + } finally { + lock.unlock(); + } + request.setHost(host); + request.setPort(port); + request.addHostHeader(); + return request; + } + + private void sendMessageToCallbacks(List callbacks, Object message) { + callbacks.forEach(callback -> sendMessageToCallback(callback, message)); + } + + private void sendMessageToCallback(String callback, Object message) { + getContext().getReference(callback).sendMessage(message); + } } diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/ChannelUtils.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/ChannelUtils.java index cfad05a3..ef40d86d 100644 --- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/ChannelUtils.java +++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/util/ChannelUtils.java @@ -16,13 +16,14 @@ */ package com.robo4j.socket.http.util; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.SocketException; import com.robo4j.socket.http.channel.DatagramConnectionType; import com.robo4j.socket.http.channel.SelectionKeyHandler; import com.robo4j.socket.http.units.ClientContext; import com.robo4j.socket.http.units.ServerContext; import com.robo4j.socket.http.units.SocketContext; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.net.DatagramSocket; @@ -44,147 +45,143 @@ * @author Miro Wengner (@miragemiko) */ public final class ChannelUtils { - /** - * reading buffer - * - * @param channel - * byte channel - * @param buffer - * buffer - * @return read bytes - * @throws IOException - * exception - */ - public static int readBuffer(ByteChannel channel, ByteBuffer buffer) throws IOException { - int numberRead = channel.read(buffer); - int position = 0; - int totalRead = numberRead; - while (numberRead >= 0 && position <= buffer.capacity()) { - numberRead = channel.read(buffer); - if (numberRead > 0) { - totalRead += numberRead; - } - position++; - } - return totalRead; - } - - /** - * writing to channel buffer - * - * @param channel - * byte channel - * @param buffer - * buffer - * @return written bytes - * @throws IOException - * exception - */ - public static int writeBuffer(ByteChannel channel, ByteBuffer buffer) throws IOException { - int numberWritten = 0; - int totalWritten = numberWritten; - - while (numberWritten >= 0 && buffer.hasRemaining()) { - numberWritten = channel.write(buffer); - totalWritten += numberWritten; - } - return totalWritten; - } - - public static SelectionKey registerSelectionKey(AbstractSelectableChannel result) { - try { - final Selector selector = Selector.open(); - return result.register(selector, SelectionKey.OP_ACCEPT); - } catch (Exception e) { - SimpleLoggingUtil.error(ChannelUtils.class, "resister selection key", e); - throw new SocketException("resister selection key", e); - } - } - - public static SelectionKey registerDatagramSelectionKey(AbstractSelectableChannel result) { - try { - final Selector selector = Selector.open(); - return result.register(selector, SelectionKey.OP_READ, SelectionKey.OP_WRITE); - } catch (Exception e) { - SimpleLoggingUtil.error(ChannelUtils.class, "resister selection key", e); - throw new SocketException("resister selection key", e); - } - } - - public static ServerSocketChannel initServerSocketChannel(ServerContext context) { - try { - final ServerSocketChannel result = ServerSocketChannel.open(); - result.bind(new InetSocketAddress(context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT))); - result.configureBlocking(false); - return result; - } catch (Exception e) { - SimpleLoggingUtil.error(ChannelUtils.class, "init server socket channel", e); - throw new SocketException("init server socket channel", e); - } - } - - public static DatagramChannel initDatagramChannel(DatagramConnectionType connectionType, SocketContext context){ - try { - final DatagramChannel result = DatagramChannel.open(); - SocketAddress address = getSocketAddressByContext(context); - switch (connectionType){ - case SERVER: - DatagramSocket socket = result.socket(); - result.configureBlocking(false); - socket.bind(address); - break; - case CLIENT: - result.connect(address); - break; - default: - throw new SocketException("int not supported: " + connectionType); - } - return result; - - } catch (Exception e){ - SimpleLoggingUtil.error(ChannelUtils.class, "init datagram socket channel", e); - throw new SocketException("init datagram socket channel", e); - } - } - - public static SocketAddress getSocketAddressByContext(SocketContext context){ - if(context instanceof ClientContext){ - final String clientHost = context.getPropertySafe(String.class, PROPERTY_HOST); - final int clientPort = context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT); - return new InetSocketAddress(clientHost,clientPort); - } else if (context instanceof ServerContext){ - final int serverPort = context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT); - return new InetSocketAddress(serverPort); - } else { - throw new SocketException("invalid context" + context); - } - } - - public static int getReadyChannelBySelectionKey(SelectionKey key) { - return getReadyChannelBySelectionKey(key, 0L); - } - - public static int getReadyChannelBySelectionKey(SelectionKey key, long timeout) { - try { - return key.selector().select(timeout); - } catch (Exception e) { - SimpleLoggingUtil.error(ChannelUtils.class, "get ready channel by selection key", e); - throw new SocketException("get ready channel by selection key", e); - } - } - - public static void handleWriteChannelAndBuffer(String message, ByteChannel channel, ByteBuffer buffer) { - try { - ChannelUtils.writeBuffer(channel, buffer); - } catch (Exception e) { - throw new SocketException(message, e); - } finally { - buffer.clear(); - } - } - - public static void handleSelectorHandler(SelectionKeyHandler handler) { - handler.handle(); - } + private static final Logger LOGGER = LoggerFactory.getLogger(ChannelUtils.class); + + /** + * reading buffer + * + * @param channel byte channel + * @param buffer buffer + * @return read bytes + * @throws IOException exception + */ + public static int readBuffer(ByteChannel channel, ByteBuffer buffer) throws IOException { + int numberRead = channel.read(buffer); + int position = 0; + int totalRead = numberRead; + while (numberRead >= 0 && position <= buffer.capacity()) { + numberRead = channel.read(buffer); + if (numberRead > 0) { + totalRead += numberRead; + } + position++; + } + return totalRead; + } + + /** + * writing to channel buffer + * + * @param channel byte channel + * @param buffer buffer + * @return written bytes + * @throws IOException exception + */ + public static int writeBuffer(ByteChannel channel, ByteBuffer buffer) throws IOException { + int numberWritten = 0; + int totalWritten = numberWritten; + + while (numberWritten >= 0 && buffer.hasRemaining()) { + numberWritten = channel.write(buffer); + totalWritten += numberWritten; + } + return totalWritten; + } + + public static SelectionKey registerSelectionKey(AbstractSelectableChannel result) { + try { + final Selector selector = Selector.open(); + return result.register(selector, SelectionKey.OP_ACCEPT); + } catch (Exception e) { + LOGGER.error("resister selection key:{}", e.getMessage(), e); + throw new SocketException("resister selection key", e); + } + } + + public static SelectionKey registerDatagramSelectionKey(AbstractSelectableChannel result) { + try { + final Selector selector = Selector.open(); + return result.register(selector, SelectionKey.OP_READ, SelectionKey.OP_WRITE); + } catch (Exception e) { + LOGGER.error("resister selection key:{}", e.getMessage(), e); + throw new SocketException("resister selection key", e); + } + } + + public static ServerSocketChannel initServerSocketChannel(ServerContext context) { + try { + final ServerSocketChannel result = ServerSocketChannel.open(); + result.bind(new InetSocketAddress(context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT))); + result.configureBlocking(false); + return result; + } catch (Exception e) { + LOGGER.error("init server socket channel:{}", e.getMessage(), e); + throw new SocketException("init server socket channel", e); + } + } + + public static DatagramChannel initDatagramChannel(DatagramConnectionType connectionType, SocketContext context) { + try { + final DatagramChannel result = DatagramChannel.open(); + SocketAddress address = getSocketAddressByContext(context); + switch (connectionType) { + case SERVER: + DatagramSocket socket = result.socket(); + result.configureBlocking(false); + socket.bind(address); + break; + case CLIENT: + result.connect(address); + break; + default: + throw new SocketException("int not supported: " + connectionType); + } + return result; + + } catch (Exception e) { + LOGGER.error("init datagram socket channel:{}", e.getMessage(), e); + throw new SocketException("init datagram socket channel", e); + } + } + + public static SocketAddress getSocketAddressByContext(SocketContext context) { + if (context instanceof ClientContext) { + final String clientHost = context.getPropertySafe(String.class, PROPERTY_HOST); + final int clientPort = context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT); + return new InetSocketAddress(clientHost, clientPort); + } else if (context instanceof ServerContext) { + final int serverPort = context.getPropertySafe(Integer.class, PROPERTY_SOCKET_PORT); + return new InetSocketAddress(serverPort); + } else { + throw new SocketException("invalid context" + context); + } + } + + public static int getReadyChannelBySelectionKey(SelectionKey key) { + return getReadyChannelBySelectionKey(key, 0L); + } + + public static int getReadyChannelBySelectionKey(SelectionKey key, long timeout) { + try { + return key.selector().select(timeout); + } catch (Exception e) { + LOGGER.error("get ready channel by selection key:{}", e.getMessage(), e); + throw new SocketException("get ready channel by selection key", e); + } + } + + public static void handleWriteChannelAndBuffer(String message, ByteChannel channel, ByteBuffer buffer) { + try { + ChannelUtils.writeBuffer(channel, buffer); + } catch (Exception e) { + throw new SocketException(message, e); + } finally { + buffer.clear(); + } + } + + public static void handleSelectorHandler(SelectionKeyHandler handler) { + handler.handle(); + } } diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java index b720401d..337bef0a 100644 --- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java +++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java @@ -16,10 +16,13 @@ */ package com.robo4j.socket.http.test.units; -import com.robo4j.*; +import com.robo4j.AttributeDescriptor; +import com.robo4j.LifecycleState; +import com.robo4j.RoboBuilder; +import com.robo4j.RoboContext; +import com.robo4j.RoboReference; import com.robo4j.configuration.Configuration; import com.robo4j.configuration.ConfigurationBuilder; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.HttpMethod; import com.robo4j.socket.http.HttpVersion; import com.robo4j.socket.http.message.HttpDecoratedRequest; @@ -36,13 +39,17 @@ import org.slf4j.Logger; import org.slf4j.LoggerFactory; -import java.util.concurrent.CountDownLatch; import java.util.concurrent.ExecutionException; import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeoutException; -import static com.robo4j.socket.http.util.RoboHttpUtils.*; -import static org.junit.jupiter.api.Assertions.*; +import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_HOST; +import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_SOCKET_PORT; +import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_TARGET; +import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_UNIT_PATHS_CONFIG; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; /** * Dynamic HttpUnit request/method configuration @@ -51,6 +58,7 @@ * @author Miro Wengner (@miragemiko) */ class RoboHttpDynamicTests { + private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpDynamicTests.class); private static final int TIMEOUT = 20; private static final TimeUnit TIME_UNIT = TimeUnit.HOURS; private static final String ID_HTTP_SERVER = "http"; @@ -60,7 +68,6 @@ class RoboHttpDynamicTests { private static final int MESSAGES_NUMBER = 42; private static final String HOST_SYSTEM = "localhost"; static final String JSON_STRING = "{\"value\":\"stop\"}"; - private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpDynamicTests.class); private static final String DECORATED_PRODUCER = "decoratedProducer"; /** @@ -199,8 +206,8 @@ private RoboContext getClientRoboSystem() throws Exception { private static R getAttributeOrTimeout(RoboReference roboReference, AttributeDescriptor attributeDescriptor) throws InterruptedException, ExecutionException, TimeoutException { var attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT, TimeUnit.MINUTES); if (attribute == null) { - SimpleLoggingUtil.error(RoboHttpDynamicTests.class, "roboReference:" + roboReference.getId() + ", no attribute:" + attributeDescriptor.getAttributeName()); attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT, TimeUnit.MINUTES); + LOGGER.error("roboReference:{}, no attribute:{}", roboReference.getId(), attributeDescriptor.getAttributeName()); } return attribute; } From 24d7e218e3d4ef748f72d738bcd366a4571dbdc4 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Wed, 9 Oct 2024 22:34:47 +0200 Subject: [PATCH 12/13] [69] more removal --- .../robo4j/units/lego/InfraSensorUnit.java | 7 +- .../com/robo4j/units/lego/SimpleTankUnit.java | 268 ++++++++-------- .../robo4j/units/lego/SingleMotorUnit.java | 99 +++--- .../robo4j/units/lego/SonicSensorUnit.java | 145 ++++----- .../lego/controller/GripperController.java | 135 ++++---- .../controller/InfraPlatformController.java | 23 +- .../hw/lego/wrapper/LcdTestWrapper.java | 12 +- .../AccelerometerLSM303Unit.java | 298 +++++++++--------- .../units/rpi/camera/RaspistillUnit.java | 132 ++++---- 9 files changed, 557 insertions(+), 562 deletions(-) diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/InfraSensorUnit.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/InfraSensorUnit.java index 48676ddf..1e0aaa01 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/InfraSensorUnit.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/InfraSensorUnit.java @@ -26,9 +26,10 @@ import com.robo4j.hw.lego.enums.SensorTypeEnum; import com.robo4j.hw.lego.provider.SensorProvider; import com.robo4j.hw.lego.wrapper.SensorWrapper; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.infra.InfraSensorEnum; import com.robo4j.units.lego.infra.InfraSensorMessage; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; @@ -40,13 +41,13 @@ * @author Miroslav Wengner (@miragemiko) */ public class InfraSensorUnit extends RoboUnit { - public static final String PROPERTY_SENSOR_PORT = "sensorPort"; public static final String PROPERTY_TARGET = "target"; public static final String PROPERTY_SCAN_INIT_DELAY = "scanInitDelay"; public static final String PROPERTY_SCAN_PERIOD = "scanPeriod"; public static final int VALUE_SCAN_INIT_DELAY = 1000; public static final int VALUE_SCAN_PERIOD = 800; + private static final Logger LOGGER = LoggerFactory.getLogger(InfraSensorUnit.class); private volatile AtomicBoolean active = new AtomicBoolean(); private ILegoSensor sensor; private String target; @@ -93,7 +94,7 @@ private void processMessage(String message){ stopMeasurement(); break; default: - SimpleLoggingUtil.error(getClass(), String.format("not supported value: %s", message)); + LOGGER.warn("not supported value: {}", message); } } diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SimpleTankUnit.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SimpleTankUnit.java index ea8a7a3c..b675af75 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SimpleTankUnit.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SimpleTankUnit.java @@ -26,10 +26,11 @@ import com.robo4j.hw.lego.enums.MotorTypeEnum; import com.robo4j.hw.lego.provider.MotorProvider; import com.robo4j.hw.lego.wrapper.MotorWrapper; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.enums.MotorRotationEnum; import com.robo4j.units.lego.platform.LegoPlatformMessage; import com.robo4j.units.lego.utils.LegoUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; @@ -42,139 +43,134 @@ */ public class SimpleTankUnit extends AbstractMotorUnit implements RoboReference { - public static final String PROPERTY_SPEED = "speed"; - public static final String PROPERTY_LEFT_MOTOR_PORT = "leftMotorPort"; - public static final String PROPERTY_LEFT_MOTOR_TYPE = "leftMotorType"; - public static final String PROPERTY_RIGHT_MOTOR_PORT = "rightMotorPort"; - public static final String PROPERTY_RIGHT_MOTOR_TYPE = "rightMotorType"; - /* test visible */ - protected volatile ILegoMotor rightMotor; - protected volatile ILegoMotor leftMotor; - private volatile int speed; - - - public SimpleTankUnit(RoboContext context, String id) { - super(LegoPlatformMessage.class, context, id); - } - - /** - * - * @param message - * the message received by this unit. - */ - @Override - public void onMessage(LegoPlatformMessage message) { - processPlatformMessage(message); - } - - @Override - public void shutdown() { - setState(LifecycleState.SHUTTING_DOWN); - rightMotor.close(); - leftMotor.close(); - setState(LifecycleState.SHUTDOWN); - } - - /** - * - * @param configuration - * the {@link Configuration} provided. - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - setState(LifecycleState.UNINITIALIZED); - speed = configuration.getInteger(PROPERTY_SPEED, LegoPlatformMessage.DEFAULT_SPEED); - String leftMotorPort = configuration.getString(PROPERTY_LEFT_MOTOR_PORT, AnalogPortEnum.B.getType()); - Character leftMotorType = configuration.getCharacter(PROPERTY_LEFT_MOTOR_TYPE, MotorTypeEnum.NXT.getType()); - String rightMotorPort = configuration.getString(PROPERTY_RIGHT_MOTOR_PORT, AnalogPortEnum.C.getType()); - Character rightMotorType = configuration.getCharacter(PROPERTY_RIGHT_MOTOR_TYPE, MotorTypeEnum.NXT.getType()); - - MotorProvider motorProvider = new MotorProvider(); - rightMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(rightMotorPort), - MotorTypeEnum.getByType(rightMotorType)); - rightMotor.setSpeed(speed); - leftMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(leftMotorPort), - MotorTypeEnum.getByType(leftMotorType)); - leftMotor.setSpeed(speed); - - setState(LifecycleState.INITIALIZED); - } - - - - // Private Methods - private void processPlatformMessage(LegoPlatformMessage message) { - switch (message.getType()) { - case STOP: - executeBothEnginesStop(rightMotor, leftMotor); - break; - case MOVE: - executeBothEngines(MotorRotationEnum.FORWARD, rightMotor, leftMotor); - break; - case BACK: - executeBothEngines(MotorRotationEnum.BACKWARD, rightMotor, leftMotor); - break; - case LEFT: - executeTurn(leftMotor, rightMotor); - break; - case RIGHT: - executeTurn(rightMotor, leftMotor); - break; - case SPEED: - checkUpdateSpeed(message); - default: - SimpleLoggingUtil.error(getClass(), message.getType() + " not supported!"); - throw new LegoUnitException("PLATFORM COMMAND: " + message); - } - } - - private void checkUpdateSpeed(LegoPlatformMessage message) { - if(message.getSpeed() != speed){ - speed = message.getSpeed(); - rightMotor.setSpeed(speed); - leftMotor.setSpeed(speed); - } - } - - private boolean executeTurn(ILegoMotor... motors) { - ILegoMotor rOne = motors[LegoUtils.DEFAULT_0]; - ILegoMotor rTwo = motors[LegoUtils.DEFAULT_1]; - Future first = runEngine(rOne, MotorRotationEnum.BACKWARD); - Future second = runEngine(rTwo, MotorRotationEnum.FORWARD); - try { - return first.get() && second.get(); - } catch (InterruptedException | ExecutionException e) { - throw new LegoUnitException("executeTurnByCycles error: ", e); - } - } - - private boolean executeBothEngines(MotorRotationEnum rotation, ILegoMotor... motors) { - Future motorLeft = runEngine(motors[LegoUtils.DEFAULT_0], rotation); - Future motorRight = runEngine(motors[LegoUtils.DEFAULT_1], rotation); - - try { - return motorLeft.get() && motorRight.get(); - } catch (InterruptedException | ExecutionException e) { - throw new LegoUnitException("BothEnginesByCycles error: ", e); - } - } - - private boolean executeBothEnginesStop(ILegoMotor... motors) { - Future motorLeft = executeEngineStop(motors[LegoUtils.DEFAULT_0]); - Future motorRight = executeEngineStop(motors[LegoUtils.DEFAULT_1]); - try { - return motorLeft.get() && motorRight.get(); - } catch (InterruptedException | ExecutionException e) { - throw new LegoUnitException("executeBothEnginesStop error: ", e); - } - } - - private Future executeEngineStop(ILegoMotor motor) { - return getContext().getScheduler().submit(() -> { - motor.stop(); - return motor.isMoving(); - }); - } + public static final String PROPERTY_SPEED = "speed"; + public static final String PROPERTY_LEFT_MOTOR_PORT = "leftMotorPort"; + public static final String PROPERTY_LEFT_MOTOR_TYPE = "leftMotorType"; + public static final String PROPERTY_RIGHT_MOTOR_PORT = "rightMotorPort"; + public static final String PROPERTY_RIGHT_MOTOR_TYPE = "rightMotorType"; + private static final Logger LOGGER = LoggerFactory.getLogger(SimpleTankUnit.class); + /* test visible */ + protected volatile ILegoMotor rightMotor; + protected volatile ILegoMotor leftMotor; + private volatile int speed; + + + public SimpleTankUnit(RoboContext context, String id) { + super(LegoPlatformMessage.class, context, id); + } + + /** + * @param message the message received by this unit. + */ + @Override + public void onMessage(LegoPlatformMessage message) { + processPlatformMessage(message); + } + + @Override + public void shutdown() { + setState(LifecycleState.SHUTTING_DOWN); + rightMotor.close(); + leftMotor.close(); + setState(LifecycleState.SHUTDOWN); + } + + /** + * @param configuration the {@link Configuration} provided. + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + setState(LifecycleState.UNINITIALIZED); + speed = configuration.getInteger(PROPERTY_SPEED, LegoPlatformMessage.DEFAULT_SPEED); + String leftMotorPort = configuration.getString(PROPERTY_LEFT_MOTOR_PORT, AnalogPortEnum.B.getType()); + Character leftMotorType = configuration.getCharacter(PROPERTY_LEFT_MOTOR_TYPE, MotorTypeEnum.NXT.getType()); + String rightMotorPort = configuration.getString(PROPERTY_RIGHT_MOTOR_PORT, AnalogPortEnum.C.getType()); + Character rightMotorType = configuration.getCharacter(PROPERTY_RIGHT_MOTOR_TYPE, MotorTypeEnum.NXT.getType()); + + MotorProvider motorProvider = new MotorProvider(); + rightMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(rightMotorPort), + MotorTypeEnum.getByType(rightMotorType)); + rightMotor.setSpeed(speed); + leftMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(leftMotorPort), + MotorTypeEnum.getByType(leftMotorType)); + leftMotor.setSpeed(speed); + + setState(LifecycleState.INITIALIZED); + } + + + // Private Methods + private void processPlatformMessage(LegoPlatformMessage message) { + switch (message.getType()) { + case STOP: + executeBothEnginesStop(rightMotor, leftMotor); + break; + case MOVE: + executeBothEngines(MotorRotationEnum.FORWARD, rightMotor, leftMotor); + break; + case BACK: + executeBothEngines(MotorRotationEnum.BACKWARD, rightMotor, leftMotor); + break; + case LEFT: + executeTurn(leftMotor, rightMotor); + break; + case RIGHT: + executeTurn(rightMotor, leftMotor); + break; + case SPEED: + checkUpdateSpeed(message); + default: + LOGGER.error("not supported!:{}", message.getType()); + throw new LegoUnitException("PLATFORM COMMAND: " + message); + } + } + + private void checkUpdateSpeed(LegoPlatformMessage message) { + if (message.getSpeed() != speed) { + speed = message.getSpeed(); + rightMotor.setSpeed(speed); + leftMotor.setSpeed(speed); + } + } + + private boolean executeTurn(ILegoMotor... motors) { + ILegoMotor rOne = motors[LegoUtils.DEFAULT_0]; + ILegoMotor rTwo = motors[LegoUtils.DEFAULT_1]; + Future first = runEngine(rOne, MotorRotationEnum.BACKWARD); + Future second = runEngine(rTwo, MotorRotationEnum.FORWARD); + try { + return first.get() && second.get(); + } catch (InterruptedException | ExecutionException e) { + throw new LegoUnitException("executeTurnByCycles error: ", e); + } + } + + private boolean executeBothEngines(MotorRotationEnum rotation, ILegoMotor... motors) { + Future motorLeft = runEngine(motors[LegoUtils.DEFAULT_0], rotation); + Future motorRight = runEngine(motors[LegoUtils.DEFAULT_1], rotation); + + try { + return motorLeft.get() && motorRight.get(); + } catch (InterruptedException | ExecutionException e) { + throw new LegoUnitException("BothEnginesByCycles error: ", e); + } + } + + private boolean executeBothEnginesStop(ILegoMotor... motors) { + Future motorLeft = executeEngineStop(motors[LegoUtils.DEFAULT_0]); + Future motorRight = executeEngineStop(motors[LegoUtils.DEFAULT_1]); + try { + return motorLeft.get() && motorRight.get(); + } catch (InterruptedException | ExecutionException e) { + throw new LegoUnitException("executeBothEnginesStop error: ", e); + } + } + + private Future executeEngineStop(ILegoMotor motor) { + return getContext().getScheduler().submit(() -> { + motor.stop(); + return motor.isMoving(); + }); + } } diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SingleMotorUnit.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SingleMotorUnit.java index 2365444e..8cff25d8 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SingleMotorUnit.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SingleMotorUnit.java @@ -26,70 +26,67 @@ import com.robo4j.hw.lego.enums.MotorTypeEnum; import com.robo4j.hw.lego.provider.MotorProvider; import com.robo4j.hw.lego.wrapper.MotorWrapper; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.enums.MotorRotationEnum; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ public class SingleMotorUnit extends AbstractMotorUnit implements RoboReference { - protected volatile ILegoMotor motor; + protected volatile ILegoMotor motor; - public SingleMotorUnit(RoboContext context, String id) { - super(MotorRotationEnum.class, context, id); - } + private static final Logger LOGGER = LoggerFactory.getLogger(SingleMotorUnit.class); - /** - * - * @param message - * the message received by this unit. - * - */ - @Override - public void onMessage(MotorRotationEnum message) { - processPlatformMessage(message); - } + public SingleMotorUnit(RoboContext context, String id) { + super(MotorRotationEnum.class, context, id); + } - @Override - public void shutdown() { - setState(LifecycleState.SHUTTING_DOWN); - motor.close(); - setState(LifecycleState.SHUTDOWN); - } + /** + * @param message the message received by this unit. + */ + @Override + public void onMessage(MotorRotationEnum message) { + processPlatformMessage(message); + } - /** - * - * @param configuration - * the {@link Configuration} provided. - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - setState(LifecycleState.UNINITIALIZED); + @Override + public void shutdown() { + setState(LifecycleState.SHUTTING_DOWN); + motor.close(); + setState(LifecycleState.SHUTDOWN); + } - String motorPort = configuration.getString("motorPort", AnalogPortEnum.A.getType()); - Character motorType = configuration.getCharacter("motorType", MotorTypeEnum.NXT.getType()); + /** + * @param configuration the {@link Configuration} provided. + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + setState(LifecycleState.UNINITIALIZED); - MotorProvider motorProvider = new MotorProvider(); - motor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(motorPort), - MotorTypeEnum.getByType(motorType)); - setState(LifecycleState.INITIALIZED); - } + String motorPort = configuration.getString("motorPort", AnalogPortEnum.A.getType()); + Character motorType = configuration.getCharacter("motorType", MotorTypeEnum.NXT.getType()); - // Private Methods - private void processPlatformMessage(MotorRotationEnum message) { - switch (message) { - case BACKWARD: - case FORWARD: - case STOP: - runEngine(motor, message); - break; - default: - SimpleLoggingUtil.error(getClass(), message + " not supported!"); - throw new LegoUnitException("single motor command: " + message); - } + MotorProvider motorProvider = new MotorProvider(); + motor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(motorPort), + MotorTypeEnum.getByType(motorType)); + setState(LifecycleState.INITIALIZED); + } - } + // Private Methods + private void processPlatformMessage(MotorRotationEnum message) { + switch (message) { + case BACKWARD: + case FORWARD: + case STOP: + runEngine(motor, message); + break; + default: + LOGGER.error("not supported!: {}", message); + throw new LegoUnitException("single motor command: " + message); + } + + } } diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SonicSensorUnit.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SonicSensorUnit.java index 1a5bbaa9..c94bfe3a 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SonicSensorUnit.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/SonicSensorUnit.java @@ -26,9 +26,10 @@ import com.robo4j.hw.lego.enums.SensorTypeEnum; import com.robo4j.hw.lego.provider.SensorProvider; import com.robo4j.hw.lego.wrapper.SensorWrapper; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.sonic.SonicSensorEnum; import com.robo4j.units.lego.sonic.SonicSensorMessage; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; @@ -40,84 +41,84 @@ * @author Miroslav Wengner (@miragemiko) */ public class SonicSensorUnit extends RoboUnit { + public static final String PROPERTY_SENSOR_PORT = "sensorPort"; + public static final String PROPERTY_TARGET = "target"; + public static final String PROPERTY_SCAN_INIT_DELAY = "scanInitDelay"; + public static final String PROPERTY_SCAN_PERIOD = "scanPeriod"; + public static final int VALUE_SCAN_INIT_DELAY = 1000; + public static final int VALUE_SCAN_PERIOD = 800; + private static final Logger LOGGER = LoggerFactory.getLogger(SonicSensorUnit.class); + private final AtomicBoolean active = new AtomicBoolean(); + private ILegoSensor sensor; + private String target; + private int scanInitialDelay; + private int scanPeriod; - public static final String PROPERTY_SENSOR_PORT = "sensorPort"; - public static final String PROPERTY_TARGET = "target"; - public static final String PROPERTY_SCAN_INIT_DELAY = "scanInitDelay"; - public static final String PROPERTY_SCAN_PERIOD = "scanPeriod"; - public static final int VALUE_SCAN_INIT_DELAY = 1000; - public static final int VALUE_SCAN_PERIOD = 800; - private final AtomicBoolean active = new AtomicBoolean(); - private ILegoSensor sensor; - private String target; - private int scanInitialDelay; - private int scanPeriod; + public SonicSensorUnit(RoboContext context, String id) { + super(String.class, context, id); + } - public SonicSensorUnit(RoboContext context, String id) { - super(String.class, context, id); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + setState(LifecycleState.UNINITIALIZED); + scanInitialDelay = configuration.getInteger(PROPERTY_SCAN_INIT_DELAY, VALUE_SCAN_INIT_DELAY); + scanPeriod = configuration.getInteger(PROPERTY_SCAN_PERIOD, VALUE_SCAN_PERIOD); + String port = configuration.getString(PROPERTY_SENSOR_PORT, null); + DigitalPortEnum sensorPort = DigitalPortEnum.getByType(port); + target = configuration.getString(PROPERTY_TARGET, null); + if (sensorPort == null) { + throw new ConfigurationException("sonic sensor port required: {S1,S2,S3,S4}"); + } + if (target == null) { + throw new ConfigurationException("sonic sensor target required"); + } + SensorProvider provider = new SensorProvider(); + sensor = new SensorWrapper<>(provider, sensorPort, SensorTypeEnum.SONIC); + setState(LifecycleState.INITIALIZED); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - setState(LifecycleState.UNINITIALIZED); - scanInitialDelay = configuration.getInteger(PROPERTY_SCAN_INIT_DELAY, VALUE_SCAN_INIT_DELAY); - scanPeriod = configuration.getInteger(PROPERTY_SCAN_PERIOD, VALUE_SCAN_PERIOD); - String port = configuration.getString(PROPERTY_SENSOR_PORT, null); - DigitalPortEnum sensorPort = DigitalPortEnum.getByType(port); - target = configuration.getString(PROPERTY_TARGET, null); - if (sensorPort == null) { - throw new ConfigurationException("sonic sensor port required: {S1,S2,S3,S4}"); - } - if (target == null) { - throw new ConfigurationException("sonic sensor target required"); - } - SensorProvider provider = new SensorProvider(); - sensor = new SensorWrapper<>(provider, sensorPort, SensorTypeEnum.SONIC); - setState(LifecycleState.INITIALIZED); - } + @Override + public void onMessage(String message) { + processMessage(message); + } - @Override - public void onMessage(String message) { - processMessage(message); - } + private void processMessage(String message) { + final SonicSensorEnum type = SonicSensorEnum.parseValue(message); + switch (type) { + case START: + active.set(true); + scheduleMeasurement(); + break; + case STOP: + stopMeasurement(); + break; + default: + LOGGER.error("not supported value: {}", message); + } + } - private void processMessage(String message){ - final SonicSensorEnum type = SonicSensorEnum.parseValue(message); - switch (type) { - case START: - active.set(true); - scheduleMeasurement(); - break; - case STOP: - stopMeasurement(); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("not supported value: %s", message)); - } - } + private void scheduleMeasurement() { + if (active.get()) { + getContext().getScheduler().scheduleAtFixedRate(this::startMeasurement, scanInitialDelay, scanPeriod, + TimeUnit.MILLISECONDS); + } + } - private void scheduleMeasurement() { - if (active.get()) { - getContext().getScheduler().scheduleAtFixedRate(this::startMeasurement, scanInitialDelay, scanPeriod, - TimeUnit.MILLISECONDS); - } - } + private void startMeasurement() { + sensor.activate(true); + String data = sensor.getData(); + sendTargetMessage(data); + sensor.activate(false); + } - private void startMeasurement() { - sensor.activate(true); - String data = sensor.getData(); - sendTargetMessage(data); - sensor.activate(false); - } + private void stopMeasurement() { + active.set(false); + sensor.activate(false); + } - private void stopMeasurement() { - active.set(false); - sensor.activate(false); - } - - private void sendTargetMessage(String distance) { - SonicSensorMessage message = new SonicSensorMessage(distance); - getContext().getReference(target).sendMessage(message); - } + private void sendTargetMessage(String distance) { + SonicSensorMessage message = new SonicSensorMessage(distance); + getContext().getReference(target).sendMessage(message); + } } diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/GripperController.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/GripperController.java index c9c4d60a..c1c69125 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/GripperController.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/GripperController.java @@ -26,8 +26,9 @@ import com.robo4j.hw.lego.enums.MotorTypeEnum; import com.robo4j.hw.lego.provider.MotorProvider; import com.robo4j.hw.lego.wrapper.MotorWrapper; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.gripper.GripperEnum; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.locks.Condition; @@ -39,76 +40,76 @@ * @author Miroslav Wengner (@miragemiko) */ public class GripperController extends RoboUnit { + public static final int ROTATION = 40; + public static final String MOTOR_PORT = "motorPort"; + public static final String MOTOR_TYPE = "motorType"; + private static final Logger LOGGER = LoggerFactory.getLogger(GripperController.class); + protected volatile ILegoMotor gripperMotor; + private volatile Lock processLock = new ReentrantLock(); + private volatile Condition processCondition = processLock.newCondition(); + private volatile AtomicBoolean active = new AtomicBoolean(); - public static final int ROTATION = 40; - public static final String MOTOR_PORT = "motorPort"; - public static final String MOTOR_TYPE = "motorType"; - protected volatile ILegoMotor gripperMotor; - private volatile Lock processLock = new ReentrantLock(); - private volatile Condition processCondition = processLock.newCondition(); - private volatile AtomicBoolean active = new AtomicBoolean(); + public GripperController(RoboContext context, String id) { + super(GripperEnum.class, context, id); + } - public GripperController(RoboContext context, String id) { - super(GripperEnum.class, context, id); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + setState(LifecycleState.UNINITIALIZED); + String motorPort = configuration.getString(MOTOR_PORT, AnalogPortEnum.A.getType()); + Character motorType = configuration.getCharacter(MOTOR_TYPE, MotorTypeEnum.NXT.getType()); - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - setState(LifecycleState.UNINITIALIZED); - String motorPort = configuration.getString(MOTOR_PORT, AnalogPortEnum.A.getType()); - Character motorType = configuration.getCharacter(MOTOR_TYPE, MotorTypeEnum.NXT.getType()); + MotorProvider motorProvider = new MotorProvider(); + gripperMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(motorPort), + MotorTypeEnum.getByType(motorType)); + setState(LifecycleState.INITIALIZED); + } - MotorProvider motorProvider = new MotorProvider(); - gripperMotor = new MotorWrapper<>(motorProvider, AnalogPortEnum.getByType(motorPort), - MotorTypeEnum.getByType(motorType)); - setState(LifecycleState.INITIALIZED); - } + @Override + public void onMessage(GripperEnum message) { + if (!active.get()) { + processMessage(message); + } + } - @Override - public void onMessage(GripperEnum message) { - if (!active.get()) { - processMessage(message); - } - } + private void processMessage(GripperEnum message) { + active.set(true); + switch (message) { + case OPEN_CLOSE: + getContext().getScheduler().execute(() -> { + rotateMotor(ROTATION); + rotateMotor(-ROTATION); + active.set(false); + }); + break; + case OPEN: + getContext().getScheduler().execute(() -> { + rotateMotor(ROTATION); + active.set(false); + }); + break; + case CLOSE: + getContext().getScheduler().execute(() -> { + rotateMotor(-ROTATION); + active.set(false); + }); + break; + default: + LOGGER.error("not implemented option: {}", message); + } + } - private void processMessage(GripperEnum message) { - active.set(true); - switch (message) { - case OPEN_CLOSE: - getContext().getScheduler().execute(() -> { - rotateMotor(ROTATION); - rotateMotor(-ROTATION); - active.set(false); - }); - break; - case OPEN: - getContext().getScheduler().execute(() -> { - rotateMotor(ROTATION); - active.set(false); - }); - break; - case CLOSE: - getContext().getScheduler().execute(() -> { - rotateMotor(-ROTATION); - active.set(false); - }); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("not implemented option: %s", message)); - } - } - - private void rotateMotor(int rotation) { - processLock.lock(); - gripperMotor.rotate(rotation); - try { - while (gripperMotor.isMoving()) { - processCondition.await(); - } - } catch (InterruptedException e) { - SimpleLoggingUtil.error(getClass(), e.getMessage()); - } finally { - processLock.unlock(); - } - } + private void rotateMotor(int rotation) { + processLock.lock(); + gripperMotor.rotate(rotation); + try { + while (gripperMotor.isMoving()) { + processCondition.await(); + } + } catch (InterruptedException e) { + LOGGER.error("error:{}", e.getMessage(), e); + } finally { + processLock.unlock(); + } + } } diff --git a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/InfraPlatformController.java b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/InfraPlatformController.java index 6da60c67..18aa5df7 100644 --- a/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/InfraPlatformController.java +++ b/robo4j-units-lego/src/main/java/com/robo4j/units/lego/controller/InfraPlatformController.java @@ -20,10 +20,11 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.lego.enums.LegoPlatformMessageTypeEnum; import com.robo4j.units.lego.infra.InfraSensorMessage; import com.robo4j.units.lego.utils.LegoUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.EnumSet; import java.util.Random; @@ -35,7 +36,6 @@ * @author Miroslav Wengner (@miragemiko) */ public class InfraPlatformController extends RoboUnit { - public static final String PROP_TARGET = "target"; public static final String PROP_DISTANCE_MIN = "distanceMin"; public static final String PROP_DISTANCE_OPTIMAL = "distanceOptimal"; @@ -44,18 +44,17 @@ public class InfraPlatformController extends RoboUnit { private static final float DISTANCE_OPTIMAL = 50f; private static final float DISTANCE_MAX = 100f; private static final Random random = new Random(); - + private static final Logger LOGGER = LoggerFactory.getLogger(InfraPlatformController.class); private static final EnumSet rotationTypes = EnumSet.of(LegoPlatformMessageTypeEnum.LEFT, LegoPlatformMessageTypeEnum.RIGHT); - private float minDistance; private float optDistance; private float maxDistance; private String target; private LegoPlatformMessageTypeEnum currentMessage = LegoPlatformMessageTypeEnum.STOP; - public InfraPlatformController(RoboContext context, String id) { - super(InfraSensorMessage.class, context, id); - } + public InfraPlatformController(RoboContext context, String id) { + super(InfraSensorMessage.class, context, id); + } @Override protected void onInitialization(Configuration configuration) throws ConfigurationException { @@ -74,9 +73,9 @@ protected void onInitialization(Configuration configuration) throws Configuratio public void onMessage(InfraSensorMessage message) { final String value = LegoUtils.parseOneElementString(message.getDistance()); final float distanceValue = LegoUtils.parseFloatStringWithInfinityDefault(value, maxDistance); - if(rotationTypes.contains(currentMessage) && distanceValue < optDistance){ - SimpleLoggingUtil.debug(getClass(), String.format("%s : adjusting platform distance: %f", getClass(), distanceValue)); - } else if(distanceValue > minDistance){ + if (rotationTypes.contains(currentMessage) && distanceValue < optDistance) { + LOGGER.info("adjusting platform distance: {}", distanceValue); + } else if (distanceValue > minDistance) { sendPlatformMessage(LegoPlatformMessageTypeEnum.MOVE); } else { int randomValue = random.nextInt(2) + 3; @@ -92,8 +91,8 @@ public void stop() { } - private void sendPlatformMessage(LegoPlatformMessageTypeEnum type){ - if(!currentMessage.equals(type)){ + private void sendPlatformMessage(LegoPlatformMessageTypeEnum type) { + if (!currentMessage.equals(type)) { currentMessage = type; getContext().getReference(target).sendMessage(type); } diff --git a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/LcdTestWrapper.java b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/LcdTestWrapper.java index b63f2b1e..bd543ff6 100644 --- a/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/LcdTestWrapper.java +++ b/robo4j-units-lego/src/test/java/com/robo4j/hw/lego/wrapper/LcdTestWrapper.java @@ -17,7 +17,8 @@ package com.robo4j.hw.lego.wrapper; import com.robo4j.hw.lego.ILcd; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Simple Lego Mindstorm LCD wrapper @@ -26,24 +27,25 @@ * @author Miro Wengner (@miragemiko) */ public class LcdTestWrapper implements ILcd { + private static final Logger LOGGER = LoggerFactory.getLogger(LcdTestWrapper.class); @Override public void initRobo4j(String title, String robotName) { - SimpleLoggingUtil.debug(getClass(), ":initRobo4j title: " + title + " name:" + robotName); + LOGGER.info("initRobo4j title:{}, name:{}", title, robotName); } @Override public void printText(int line, int increment, String text) { - SimpleLoggingUtil.debug(getClass(), ":printText line: " + line + ", text: " + text); + LOGGER.info("printText line:{}, text:{}", line, text); } @Override public void clear() { - SimpleLoggingUtil.debug(getClass(),":cleared"); + LOGGER.info("cleared"); } @Override public void printText(String text) { - SimpleLoggingUtil.debug(getClass(),":printText = " + text); + LOGGER.info("printText:{}", text); } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java index 9117cb99..451a683b 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/accelerometer/AccelerometerLSM303Unit.java @@ -16,15 +16,6 @@ */ package com.robo4j.units.rpi.accelerometer; -import java.io.IOException; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.List; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -36,157 +27,164 @@ import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.DataRate; import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.FullScale; import com.robo4j.hw.rpi.i2c.accelerometer.AccelerometerLSM303Device.PowerMode; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.hw.rpi.i2c.accelerometer.CalibratedAccelerometer; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; /** * Accelerometer unit. - * + *

* Use configuration settings to compensate for inverted axes, and for * calibration. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AccelerometerLSM303Unit extends I2CRoboUnit { - /** - * This key controls the {@link PowerMode}. Use the enum name of the power - * mode you want to use. The default is NORMAL. - */ - public static final String PROPERTY_KEY_POWER_MODE = "powerMode"; - - /** - * This key controls the {@link DataRate} to use. Use the enum name of the - * data rate you want to use. The default is HZ_10. - */ - public static final String PROPERTY_KEY_RATE = "rate"; - - /** - * This key controls which axes to enable. See - * {@link AccelerometerLSM303Device}. - * - * Default is 7. See {@link AccelerometerLSM303Device#AXIS_ENABLE_ALL}. - */ - private static final String PROPERTY_KEY_AXIS_ENABLE = "axisEnable"; - - /** - * This key controls the full scale (and thereby the sensitivity) of the - * accelerometer. See {@link FullScale}. Default is G_2. - */ - private static final String PROPERTY_KEY_FULL_SCALE = "fullScale"; - - /** - * This key controls the enabling the high res mode. Default is false. - */ - private static final String PROPERTY_KEY_ENABLE_HIGH_RES = "enableHighRes"; - - /** - * This key controls how often to read the accelerometer, in ms. Default is - * 200. - */ - private static final String PROPERTY_KEY_PERIOD = "period"; - - /** - * This attribute will provide the state of the accelerometer as a - * {@link Tuple3f}. - */ - public static final String ATTRIBUTE_NAME_STATE = "state"; - - public static final Collection> KNOWN_ATTRIBUTES = List.of(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE)); - - private final Scanner scanner = new Scanner(); - - private CalibratedFloat3DDevice accelerometer; - private Integer period; - - private volatile ScheduledFuture scannerTask; - private final List requests = new ArrayList<>(); - - private class Scanner implements Runnable { - @Override - public void run() { - try { - Tuple3f value = accelerometer.read(); - for (AccelerometerRequest request : requests) { - if (request.getPredicate().test(value)) { - notify(request.getTarget(), value); - } - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read accelerometer!", e); - } - } - - private void notify(RoboReference target, Tuple3f value) { - target.sendMessage(new AccelerometerEvent(value)); - } - } - - /** - * Constructor. - * - * @param context - * the robo context. - * @param id - * the robo unit id. - */ - public AccelerometerLSM303Unit(RoboContext context, String id) { - super(AccelerometerRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - PowerMode powerMode = PowerMode.valueOf(configuration.getString(PROPERTY_KEY_POWER_MODE, PowerMode.NORMAL.name())); - DataRate rate = DataRate.valueOf(configuration.getString(PROPERTY_KEY_RATE, DataRate.HZ_10.name())); - Integer axisEnable = configuration.getInteger(PROPERTY_KEY_AXIS_ENABLE, AccelerometerLSM303Device.AXIS_ENABLE_ALL); - FullScale fullScale = FullScale.valueOf(configuration.getString(PROPERTY_KEY_FULL_SCALE, FullScale.G_2.name())); - Boolean enableHighRes = configuration.getBoolean(PROPERTY_KEY_ENABLE_HIGH_RES, false); - Tuple3f offsets = readFloat3D(configuration.getChildConfiguration("offsets")); - Tuple3f multipliers = readFloat3D(configuration.getChildConfiguration("multipliers")); - period = configuration.getInteger(PROPERTY_KEY_PERIOD, 200); - - try { - AccelerometerLSM303Device device = new AccelerometerLSM303Device(getBus(), getAddress(), powerMode, rate, axisEnable, fullScale, - enableHighRes); - accelerometer = new CalibratedAccelerometer(device, offsets, multipliers); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - private Tuple3f readFloat3D(Configuration config) { - return new Tuple3f(config.getFloat("x", 0f), config.getFloat("y", 0f), config.getFloat("z", 0f)); - } - - @Override - public void onMessage(AccelerometerRequest message) { - super.onMessage(message); - registerRequest(message); - } - - private void registerRequest(AccelerometerRequest message) { - synchronized (this) { - requests.add(message); - } - if (scannerTask == null) { - scannerTask = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { - try { - return (R) accelerometer.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the accelerometer!", e); - } - } - return super.onGetAttribute(descriptor); - } + /** + * This key controls the {@link PowerMode}. Use the enum name of the power + * mode you want to use. The default is NORMAL. + */ + public static final String PROPERTY_KEY_POWER_MODE = "powerMode"; + + /** + * This key controls the {@link DataRate} to use. Use the enum name of the + * data rate you want to use. The default is HZ_10. + */ + public static final String PROPERTY_KEY_RATE = "rate"; + + /** + * This key controls which axes to enable. See + * {@link AccelerometerLSM303Device}. + *

+ * Default is 7. See {@link AccelerometerLSM303Device#AXIS_ENABLE_ALL}. + */ + private static final String PROPERTY_KEY_AXIS_ENABLE = "axisEnable"; + + /** + * This key controls the full scale (and thereby the sensitivity) of the + * accelerometer. See {@link FullScale}. Default is G_2. + */ + private static final String PROPERTY_KEY_FULL_SCALE = "fullScale"; + + /** + * This key controls the enabling the high res mode. Default is false. + */ + private static final String PROPERTY_KEY_ENABLE_HIGH_RES = "enableHighRes"; + + /** + * This key controls how often to read the accelerometer, in ms. Default is + * 200. + */ + private static final String PROPERTY_KEY_PERIOD = "period"; + + /** + * This attribute will provide the state of the accelerometer as a + * {@link Tuple3f}. + */ + public static final String ATTRIBUTE_NAME_STATE = "state"; + + public static final Collection> KNOWN_ATTRIBUTES = List.of(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE)); + + private static final Logger LOGGER = LoggerFactory.getLogger(AccelerometerLSM303Unit.class); + private final Scanner scanner = new Scanner(); + + private CalibratedFloat3DDevice accelerometer; + private Integer period; + + private volatile ScheduledFuture scannerTask; + private final List requests = new ArrayList<>(); + + private class Scanner implements Runnable { + @Override + public void run() { + try { + Tuple3f value = accelerometer.read(); + for (AccelerometerRequest request : requests) { + if (request.getPredicate().test(value)) { + notify(request.getTarget(), value); + } + } + } catch (IOException e) { + LOGGER.error("Failed to read accelerometer!:{}", e.getMessage(), e); + } + } + + private void notify(RoboReference target, Tuple3f value) { + target.sendMessage(new AccelerometerEvent(value)); + } + } + + /** + * Constructor. + * + * @param context the robo context. + * @param id the robo unit id. + */ + public AccelerometerLSM303Unit(RoboContext context, String id) { + super(AccelerometerRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + PowerMode powerMode = PowerMode.valueOf(configuration.getString(PROPERTY_KEY_POWER_MODE, PowerMode.NORMAL.name())); + DataRate rate = DataRate.valueOf(configuration.getString(PROPERTY_KEY_RATE, DataRate.HZ_10.name())); + Integer axisEnable = configuration.getInteger(PROPERTY_KEY_AXIS_ENABLE, AccelerometerLSM303Device.AXIS_ENABLE_ALL); + FullScale fullScale = FullScale.valueOf(configuration.getString(PROPERTY_KEY_FULL_SCALE, FullScale.G_2.name())); + Boolean enableHighRes = configuration.getBoolean(PROPERTY_KEY_ENABLE_HIGH_RES, false); + Tuple3f offsets = readFloat3D(configuration.getChildConfiguration("offsets")); + Tuple3f multipliers = readFloat3D(configuration.getChildConfiguration("multipliers")); + period = configuration.getInteger(PROPERTY_KEY_PERIOD, 200); + + try { + AccelerometerLSM303Device device = new AccelerometerLSM303Device(getBus(), getAddress(), powerMode, rate, axisEnable, fullScale, + enableHighRes); + accelerometer = new CalibratedAccelerometer(device, offsets, multipliers); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + private Tuple3f readFloat3D(Configuration config) { + return new Tuple3f(config.getFloat("x", 0f), config.getFloat("y", 0f), config.getFloat("z", 0f)); + } + + @Override + public void onMessage(AccelerometerRequest message) { + super.onMessage(message); + registerRequest(message); + } + + private void registerRequest(AccelerometerRequest message) { + synchronized (this) { + requests.add(message); + } + if (scannerTask == null) { + scannerTask = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { + try { + return (R) accelerometer.read(); + } catch (IOException e) { + LOGGER.error("Failed to read the accelerometer!:{}", e.getMessage(), e); + } + } + return super.onGetAttribute(descriptor); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java index 9be60258..3ecefeab 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/camera/RaspistillUnit.java @@ -23,7 +23,8 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.camera.RaspiDevice; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.EnumSet; import java.util.concurrent.atomic.AtomicBoolean; @@ -31,83 +32,82 @@ /** * Unit generates image by using Rapsberry Pi raspistill utility * - * @see raspistill - * documentation - * * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) + * @see raspistill + * documentation */ public class RaspistillUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(RaspistillUnit.class); + private static final EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, + LifecycleState.STARTED); + private static final String PROPERTY_TARGET = "target"; + private final AtomicBoolean continualMode = new AtomicBoolean(false); + private final AtomicBoolean cameraProgress = new AtomicBoolean(false); - private static final EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, - LifecycleState.STARTED); - private static final String PROPERTY_TARGET = "target"; - private final AtomicBoolean continualMode = new AtomicBoolean(false); - private final AtomicBoolean cameraProgress = new AtomicBoolean(false); - - private final RaspiDevice device = new RaspiDevice(); - private String target; + private final RaspiDevice device = new RaspiDevice(); + private String target; - public RaspistillUnit(RoboContext context, String id) { - super(RaspistillRequest.class, context, id); - } + public RaspistillUnit(RoboContext context, String id) { + super(RaspistillRequest.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString(PROPERTY_TARGET, null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); - } - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString(PROPERTY_TARGET, null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException(PROPERTY_TARGET); + } + } - @Override - public void onMessage(RaspistillRequest message) { - processMessage(message); - } + @Override + public void onMessage(RaspistillRequest message) { + processMessage(message); + } - private void processMessage(RaspistillRequest message) { - getContext().getScheduler().execute(() -> { - if (continualMode.get()) { - stopProgress(); - } - if (message.isActive()) { - startContinualMode(message); - } else if (cameraProgress.compareAndSet(false, true)) { - createImage(message); - } - }); - } + private void processMessage(RaspistillRequest message) { + getContext().getScheduler().execute(() -> { + if (continualMode.get()) { + stopProgress(); + } + if (message.isActive()) { + startContinualMode(message); + } else if (cameraProgress.compareAndSet(false, true)) { + createImage(message); + } + }); + } - private void stopProgress() { - continualMode.set(false); - while (cameraProgress.get()); - } + private void stopProgress() { + continualMode.set(false); + while (cameraProgress.get()) ; + } - private void startContinualMode(RaspistillRequest message) { - getContext().getScheduler().execute(() -> { - continualMode.set(true); - while (acceptedStates.contains(getState()) && continualMode.get()) { - if (cameraProgress.compareAndSet(false, true)) { - createImage(message); - } - } - }); + private void startContinualMode(RaspistillRequest message) { + getContext().getScheduler().execute(() -> { + continualMode.set(true); + while (acceptedStates.contains(getState()) && continualMode.get()) { + if (cameraProgress.compareAndSet(false, true)) { + createImage(message); + } + } + }); - } + } - private void createImage(RaspistillRequest message) { - try { - final byte[] image = device.executeCommandRaspistill(message.create()); - final RoboReference targetReference = getContext().getReference(target); - if (targetReference != null && image.length > 0) { - ImageDTO imageDTO = CameraUtil.createImageDTOBydMessageAndBytes(message, image); - targetReference.sendMessage(imageDTO); - } - cameraProgress.set(false); - } catch (Exception e) { - SimpleLoggingUtil.error(getClass(), "create image", e); - } - } + private void createImage(RaspistillRequest message) { + try { + final byte[] image = device.executeCommandRaspistill(message.create()); + final RoboReference targetReference = getContext().getReference(target); + if (targetReference != null && image.length > 0) { + ImageDTO imageDTO = CameraUtil.createImageDTOBydMessageAndBytes(message, image); + targetReference.sendMessage(imageDTO); + } + cameraProgress.set(false); + } catch (Exception e) { + LOGGER.error("create image:{}", e.getMessage(), e); + } + } } From c8bbe1d4936980aba1d6d093748e558ef01227b7 Mon Sep 17 00:00:00 2001 From: Miro Wengner Date: Wed, 9 Oct 2024 22:57:05 +0200 Subject: [PATCH 13/13] [69] final cleanup --- .../com/robo4j/logging/SimpleLoggingUtil.java | 67 -- robo4j-core/src/main/java/module-info.java | 1 - .../rpi/http/camera/ImageDecoratorUnit.java | 6 +- .../src/main/java/module-info.java | 1 + .../CameraImageProducerDesTestUnit.java | 149 ++--- .../com/robo4j/units/rpi/gps/MtkGPSUnit.java | 324 ++++----- .../robo4j/units/rpi/gps/TitanGPSUnit.java | 204 +++--- .../robo4j/units/rpi/gyro/GyroL3GD20Unit.java | 389 ++++++----- .../com/robo4j/units/rpi/imu/Bno080Unit.java | 178 ++--- .../units/rpi/imu/DataEventListenerUnit.java | 18 +- .../rpi/imu/VectorEventListenerUnit.java | 55 +- .../units/rpi/lcd/AdafruitButtonUnit.java | 164 ++--- .../robo4j/units/rpi/lcd/AdafruitLcdUnit.java | 303 ++++----- .../rpi/led/AbstractI2CBackpackUnit.java | 54 +- .../rpi/led/AdafruitAlphanumericUnit.java | 98 +-- .../units/rpi/lidarlite/LaserScanner.java | 627 +++++++++--------- .../robo4j/units/rpi/pad/LF710PadUnit.java | 15 +- .../units/rpi/pwm/MC33926HBridgeUnit.java | 174 ++--- .../units/rpi/pwm/PCA9685ServoUnit.java | 220 +++--- .../rpi/roboclaw/RoboClawRCTankUnit.java | 138 ++-- 20 files changed, 1557 insertions(+), 1628 deletions(-) delete mode 100644 robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java diff --git a/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java b/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java deleted file mode 100644 index e54abee4..00000000 --- a/robo4j-core/src/main/java/com/robo4j/logging/SimpleLoggingUtil.java +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright (c) 2014, 2024, Marcus Hirt, Miroslav Wengner - * - * Robo4J is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Robo4J is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Robo4J. If not, see . - */ -package com.robo4j.logging; - -import com.robo4j.util.StringConstants; - -import java.util.logging.Level; -import java.util.logging.Logger; -import java.util.stream.Stream; - -/** - * Simple toolkit for logging. - * - * @author Marcus Hirt (@hirt) - * @author Miroslav Wengner (@miragemiko) - */ -@Deprecated -public final class SimpleLoggingUtil { - // TODO(Marcus/Sep 6, 2017): Decide if we want to kill this... - public static void print(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message); - } - - public static void debug(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.FINE, message); - } - - public static void debug(Class clazz, String... message) { - debug(clazz, clazz.getSimpleName() + " : " - + Stream.of(message).reduce(StringConstants.EMPTY, (l, r) -> l.concat(StringConstants.SPACE).concat(r))); - } - - public static void debug(Class clazz, String message, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); - } - - public static void info(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message); - } - - public static void info(Class clazz, String message, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.INFO, message, e); - } - - public static void error(Class clazz, String message) { - Logger.getLogger(clazz.getName()).log(Level.SEVERE, message); - } - - public static void error(Class clazz, String string, Throwable e) { - Logger.getLogger(clazz.getName()).log(Level.SEVERE, string, e); - } - -} diff --git a/robo4j-core/src/main/java/module-info.java b/robo4j-core/src/main/java/module-info.java index 58faca17..a3274969 100644 --- a/robo4j-core/src/main/java/module-info.java +++ b/robo4j-core/src/main/java/module-info.java @@ -26,7 +26,6 @@ exports com.robo4j; exports com.robo4j.util; exports com.robo4j.configuration; - exports com.robo4j.logging; exports com.robo4j.reflect; exports com.robo4j.scheduler; exports com.robo4j.net; diff --git a/robo4j-units-rpi-http/src/main/java/com/robo4j/units/rpi/http/camera/ImageDecoratorUnit.java b/robo4j-units-rpi-http/src/main/java/com/robo4j/units/rpi/http/camera/ImageDecoratorUnit.java index 5fb24d66..06651478 100644 --- a/robo4j-units-rpi-http/src/main/java/com/robo4j/units/rpi/http/camera/ImageDecoratorUnit.java +++ b/robo4j-units-rpi-http/src/main/java/com/robo4j/units/rpi/http/camera/ImageDecoratorUnit.java @@ -21,12 +21,13 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.codec.CameraMessage; import com.robo4j.socket.http.enums.SystemPath; import com.robo4j.socket.http.units.ClientMessageWrapper; import com.robo4j.socket.http.util.HttpPathUtils; import com.robo4j.socket.http.util.JsonUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.Objects; import java.util.concurrent.atomic.AtomicInteger; @@ -41,6 +42,7 @@ */ @CriticalSectionTrait public class ImageDecoratorUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(ImageDecoratorUnit.class); private static final String PROPERTY_TARGET = "target"; private final AtomicInteger imageNumber = new AtomicInteger(0); @@ -69,7 +71,7 @@ public void onMessage(CameraImageDTO image) { String.valueOf(imageNumber.incrementAndGet()), imageBase64); final ClientMessageWrapper resultMessage = new ClientMessageWrapper( HttpPathUtils.toPath(SystemPath.UNITS.getPath(), httpTarget), CameraMessage.class, cameraMessage); - SimpleLoggingUtil.debug(ImageDecoratorUnit.class, "image target: " + target + " resultMessage: " + resultMessage.getPath()); + LOGGER.info("image target:{},resultMessage:{}", target, resultMessage.getPath()); getContext().getReference(target).sendMessage(resultMessage); } diff --git a/robo4j-units-rpi-http/src/main/java/module-info.java b/robo4j-units-rpi-http/src/main/java/module-info.java index a3c28603..026860b0 100644 --- a/robo4j-units-rpi-http/src/main/java/module-info.java +++ b/robo4j-units-rpi-http/src/main/java/module-info.java @@ -1,5 +1,6 @@ module robo4j.units.rpi.http { requires robo4j.http; + requires org.slf4j; exports com.robo4j.units.rpi.http.camera to robo4j.core; } \ No newline at end of file diff --git a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerDesTestUnit.java b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerDesTestUnit.java index 8e214127..a5e53004 100644 --- a/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerDesTestUnit.java +++ b/robo4j-units-rpi-http/src/test/java/com/robo4j/units/rpi/http/camera/CameraImageProducerDesTestUnit.java @@ -23,13 +23,14 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.socket.http.codec.CameraMessage; import com.robo4j.socket.http.enums.SystemPath; import com.robo4j.socket.http.units.ClientMessageWrapper; import com.robo4j.socket.http.util.HttpPathUtils; import com.robo4j.socket.http.util.JsonUtil; import com.robo4j.util.StreamUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.util.EnumSet; import java.util.concurrent.CountDownLatch; @@ -43,85 +44,85 @@ public class CameraImageProducerDesTestUnit extends RoboUnit { - public static final String ATTRIBUTE_NUMBER_OF_SENT_IMAGES_NAME = "numberOfSentImages"; - public static final String ATTR_TOTAL_IMAGES = "numberOfImages"; - public static final String ATTR_GENERATED_IMAGES_LATCH = "generatedImagesLatch"; - public static final DefaultAttributeDescriptor DESCRIPTOR_GENERATED_IMAGES_LATCH = DefaultAttributeDescriptor - .create(CountDownLatch.class, ATTR_GENERATED_IMAGES_LATCH); - public static final AttributeDescriptor DESCRIPTOR_TOTAL_IMAGES = new DefaultAttributeDescriptor<>(Integer.class, - CameraImageProducerDesTestUnit.ATTR_TOTAL_IMAGES); - protected static final String IMAGE_ENCODING = "jpg"; + public static final String ATTRIBUTE_NUMBER_OF_SENT_IMAGES_NAME = "numberOfSentImages"; + public static final String ATTR_TOTAL_IMAGES = "numberOfImages"; + public static final String ATTR_GENERATED_IMAGES_LATCH = "generatedImagesLatch"; + public static final DefaultAttributeDescriptor DESCRIPTOR_GENERATED_IMAGES_LATCH = DefaultAttributeDescriptor + .create(CountDownLatch.class, ATTR_GENERATED_IMAGES_LATCH); + public static final AttributeDescriptor DESCRIPTOR_TOTAL_IMAGES = new DefaultAttributeDescriptor<>(Integer.class, + CameraImageProducerDesTestUnit.ATTR_TOTAL_IMAGES); + protected static final String IMAGE_ENCODING = "jpg"; + private static final Logger LOGGER = LoggerFactory.getLogger(CameraImageProducerDesTestUnit.class); + protected final AtomicBoolean progress = new AtomicBoolean(false); + protected String target; + protected String httpTarget; + protected String fileName; + private volatile AtomicInteger counter = new AtomicInteger(0); + protected volatile CountDownLatch generatedImagesLatch; + private Integer numberOfImages; - protected final AtomicBoolean progress = new AtomicBoolean(false); - protected String target; - protected String httpTarget; - protected String fileName; - private volatile AtomicInteger counter = new AtomicInteger(0); - protected volatile CountDownLatch generatedImagesLatch; - private Integer numberOfImages; + public CameraImageProducerDesTestUnit(RoboContext context, String id) { + super(Boolean.class, context, id); + } - public CameraImageProducerDesTestUnit(RoboContext context, String id) { - super(Boolean.class, context, id); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + target = configuration.getString("target", null); + httpTarget = configuration.getString("httpTarget", null); + fileName = configuration.getString("fileName", null); + numberOfImages = configuration.getInteger(ATTR_TOTAL_IMAGES, null); + generatedImagesLatch = new CountDownLatch(numberOfImages); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - target = configuration.getString("target", null); - httpTarget = configuration.getString("httpTarget", null); - fileName = configuration.getString("fileName", null); - numberOfImages = configuration.getInteger(ATTR_TOTAL_IMAGES, null); - generatedImagesLatch = new CountDownLatch(numberOfImages); - } + @Override + public void onMessage(Boolean message) { + if (message) { + LOGGER.info("message:{}", message); + createImage(counter.get()); + } + } - @Override - public void onMessage(Boolean message) { - if (message) { - SimpleLoggingUtil.debug(getClass(), "message: " + message); - createImage(counter.get()); - } - } + @Override + public void start() { + EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, LifecycleState.STARTED); + getContext().getScheduler().execute(() -> { + while (acceptedStates.contains(getState())) { + if (progress.compareAndSet(false, true) && counter.getAndIncrement() < numberOfImages) { + createImage(counter.get()); + } + } + }); + } - @Override - public void start() { - EnumSet acceptedStates = EnumSet.of(LifecycleState.STARTING, LifecycleState.STARTED); - getContext().getScheduler().execute(() -> { - while (acceptedStates.contains(getState())) { - if (progress.compareAndSet(false, true) && counter.getAndIncrement() < numberOfImages) { - createImage(counter.get()); - } - } - }); - } + @SuppressWarnings("unchecked") + @Override + protected synchronized R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Integer.class) { + if (descriptor.getAttributeName().equals(ATTR_TOTAL_IMAGES)) { + return (R) numberOfImages; + } else if (descriptor.getAttributeName().equals(ATTRIBUTE_NUMBER_OF_SENT_IMAGES_NAME)) { + return (R) Integer.valueOf(counter.get()); + } + } + if (descriptor.getAttributeName().equals(ATTR_GENERATED_IMAGES_LATCH) && descriptor.getAttributeType() == CountDownLatch.class) { + return (R) generatedImagesLatch; + } + return super.onGetAttribute(descriptor); + } - @SuppressWarnings("unchecked") - @Override - protected synchronized R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Integer.class) { - if (descriptor.getAttributeName().equals(ATTR_TOTAL_IMAGES)) { - return (R) numberOfImages; - } else if (descriptor.getAttributeName().equals(ATTRIBUTE_NUMBER_OF_SENT_IMAGES_NAME)) { - return (R) Integer.valueOf(counter.get()); - } - } - if(descriptor.getAttributeName().equals(ATTR_GENERATED_IMAGES_LATCH) && descriptor.getAttributeType() == CountDownLatch.class ){ - return (R) generatedImagesLatch; - } - return super.onGetAttribute(descriptor); - } + protected void countDonwSentImage() { - protected void countDonwSentImage(){ + } - } - - protected void createImage(int imageNumber) { - final byte[] image = StreamUtils - .inputStreamToByteArray(Thread.currentThread().getContextClassLoader().getResourceAsStream(fileName)); - final CameraMessage cameraMessage = new CameraMessage(IMAGE_ENCODING, String.valueOf(imageNumber), - JsonUtil.toBase64String(image)); - final ClientMessageWrapper resultMessage = new ClientMessageWrapper( - HttpPathUtils.toPath(SystemPath.UNITS.getPath(), httpTarget), CameraMessage.class, cameraMessage); - getContext().getReference(target).sendMessage(resultMessage); - generatedImagesLatch.countDown(); - progress.set(false); - } + protected void createImage(int imageNumber) { + final byte[] image = StreamUtils + .inputStreamToByteArray(Thread.currentThread().getContextClassLoader().getResourceAsStream(fileName)); + final CameraMessage cameraMessage = new CameraMessage(IMAGE_ENCODING, String.valueOf(imageNumber), + JsonUtil.toBase64String(image)); + final ClientMessageWrapper resultMessage = new ClientMessageWrapper( + HttpPathUtils.toPath(SystemPath.UNITS.getPath(), httpTarget), CameraMessage.class, cameraMessage); + getContext().getReference(target).sendMessage(resultMessage); + generatedImagesLatch.countDown(); + progress.set(false); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java index 3eaa7a93..2a31b4d6 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/MtkGPSUnit.java @@ -16,16 +16,6 @@ */ package com.robo4j.units.rpi.gps; -import java.io.IOException; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.List; -import java.util.concurrent.Future; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -38,163 +28,175 @@ import com.robo4j.hw.rpi.gps.PositionEvent; import com.robo4j.hw.rpi.gps.VelocityEvent; import com.robo4j.hw.rpi.serial.gps.MTK3339GPS; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.math.geometry.Tuple3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.Collections; +import java.util.List; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; /** * Unit for getting GPS data. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MtkGPSUnit extends RoboUnit { - /** - * This key configures the approximate update interval, or how often to - * schedule reads from the serial port. - */ - public static final String PROPERTY_KEY_READ_INTERVAL = "readInterval"; - - /** - * This key configures the scheduler to use for scheduling reads. Either - * PLATFORM or INTERNAL. Use INTERNAL if the reads take too long and start - * disrupting the platform scheduler too much. - */ - public static final String PROPERTY_KEY_SCHEDULER = "scheduler"; - - /** - * Value for the scheduler key for using the platform scheduler. - */ - public static final String PROPERTY_VALUE_PLATFORM_SCHEDULER = "platform"; - - /** - * Value for the scheduler key for using the internal scheduler. - */ - public static final String PROPERTY_VALUE_INTERNAL_SCHEDULER = "internal"; - - /** - * This is the default value for the read interval. - */ - public static final int DEFAULT_READ_INTERVAL = MTK3339GPS.DEFAULT_READ_INTERVAL; - - /** - * This is the default value for the serial port. - */ - public static final String DEFAULT_SERIAL_PORT = MTK3339GPS.DEFAULT_GPS_PORT; - - /** - * This attribute will provide the state of the read interval. - */ - public static final String ATTRIBUTE_NAME_READ_INTERVAL = "readInterval"; - - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_READ_INTERVAL))); - - private MTK3339GPS mtk3339gps; - private String serialPort; - private int readInterval = DEFAULT_READ_INTERVAL; - private List listeners = new ArrayList<>(); - - // The future, if scheduled with the platform scheduler - private volatile ScheduledFuture scheduledFuture; - - private static class GPSEventListener implements GPSListener { - private RoboReference target; - - GPSEventListener(RoboReference target) { - this.target = target; - } - - @Override - public void onPosition(PositionEvent event) { - target.sendMessage(event); - } - - @Override - public void onVelocity(VelocityEvent event) { - target.sendMessage(event); - } - } - - public MtkGPSUnit(RoboContext context, String id) { - super(GPSRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - serialPort = configuration.getString("serialPort", DEFAULT_SERIAL_PORT); - readInterval = configuration.getInteger("readInterval", DEFAULT_READ_INTERVAL); - String scheduler = configuration.getString("scheduler", PROPERTY_VALUE_PLATFORM_SCHEDULER); - boolean usePlatformScheduler = PROPERTY_VALUE_PLATFORM_SCHEDULER.equals(scheduler); - - try { - mtk3339gps = new MTK3339GPS(serialPort, readInterval); - } catch (IOException e) { - throw new ConfigurationException("Could not instantiate GPS!", e); - } - if (usePlatformScheduler) { - scheduledFuture = getContext().getScheduler().scheduleAtFixedRate(() -> mtk3339gps.update(), 10, readInterval, - TimeUnit.MILLISECONDS); - } else { - mtk3339gps.start(); - } - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - return super.getAttribute(attribute); - } - - @Override - public void onMessage(GPSRequest message) { - super.onMessage(message); - RoboReference targetReference = message.getTarget(); - switch (message.getOperation()) { - case REGISTER: - register(targetReference); - break; - case UNREGISTER: - unregister(targetReference); - break; - default: - SimpleLoggingUtil.error(getClass(), "Unknown operation: " + message.getOperation()); - break; - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Integer.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_READ_INTERVAL)) { - return (R) Integer.valueOf(readInterval); - } - return super.onGetAttribute(descriptor); - } - - @Override - public void shutdown() { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - mtk3339gps.shutdown(); - super.shutdown(); - } - - // Private Methods - private synchronized void unregister(RoboReference targetReference) { - List copy = new ArrayList<>(listeners); - for (GPSEventListener listener : copy) { - if (targetReference.equals(listener.target)) { - listeners.remove(listener); - mtk3339gps.removeListener(listener); - // I guess you could theoretically have several registered to - // the same target, so let's keep checking... - } - } - } - - private synchronized void register(RoboReference targetReference) { - GPSEventListener listener = new GPSEventListener(targetReference); - listeners.add(listener); - mtk3339gps.addListener(listener); - } + /** + * This key configures the approximate update interval, or how often to + * schedule reads from the serial port. + */ + public static final String PROPERTY_KEY_READ_INTERVAL = "readInterval"; + + /** + * This key configures the scheduler to use for scheduling reads. Either + * PLATFORM or INTERNAL. Use INTERNAL if the reads take too long and start + * disrupting the platform scheduler too much. + */ + public static final String PROPERTY_KEY_SCHEDULER = "scheduler"; + + /** + * Value for the scheduler key for using the platform scheduler. + */ + public static final String PROPERTY_VALUE_PLATFORM_SCHEDULER = "platform"; + + /** + * Value for the scheduler key for using the internal scheduler. + */ + public static final String PROPERTY_VALUE_INTERNAL_SCHEDULER = "internal"; + + /** + * This is the default value for the read interval. + */ + public static final int DEFAULT_READ_INTERVAL = MTK3339GPS.DEFAULT_READ_INTERVAL; + + /** + * This is the default value for the serial port. + */ + public static final String DEFAULT_SERIAL_PORT = MTK3339GPS.DEFAULT_GPS_PORT; + + /** + * This attribute will provide the state of the read interval. + */ + public static final String ATTRIBUTE_NAME_READ_INTERVAL = "readInterval"; + + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_READ_INTERVAL))); + private static final Logger LOGGER = LoggerFactory.getLogger(MtkGPSUnit.class); + + private MTK3339GPS mtk3339gps; + private String serialPort; + private int readInterval = DEFAULT_READ_INTERVAL; + private List listeners = new ArrayList<>(); + + // The future, if scheduled with the platform scheduler + private volatile ScheduledFuture scheduledFuture; + + private static class GPSEventListener implements GPSListener { + private RoboReference target; + + GPSEventListener(RoboReference target) { + this.target = target; + } + + @Override + public void onPosition(PositionEvent event) { + target.sendMessage(event); + } + + @Override + public void onVelocity(VelocityEvent event) { + target.sendMessage(event); + } + } + + public MtkGPSUnit(RoboContext context, String id) { + super(GPSRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + serialPort = configuration.getString("serialPort", DEFAULT_SERIAL_PORT); + readInterval = configuration.getInteger("readInterval", DEFAULT_READ_INTERVAL); + String scheduler = configuration.getString("scheduler", PROPERTY_VALUE_PLATFORM_SCHEDULER); + boolean usePlatformScheduler = PROPERTY_VALUE_PLATFORM_SCHEDULER.equals(scheduler); + + try { + mtk3339gps = new MTK3339GPS(serialPort, readInterval); + } catch (IOException e) { + throw new ConfigurationException("Could not instantiate GPS!", e); + } + if (usePlatformScheduler) { + scheduledFuture = getContext().getScheduler().scheduleAtFixedRate(() -> mtk3339gps.update(), 10, readInterval, + TimeUnit.MILLISECONDS); + } else { + mtk3339gps.start(); + } + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + return super.getAttribute(attribute); + } + + @Override + public void onMessage(GPSRequest message) { + super.onMessage(message); + RoboReference targetReference = message.getTarget(); + switch (message.getOperation()) { + case REGISTER: + register(targetReference); + break; + case UNREGISTER: + unregister(targetReference); + break; + default: + LOGGER.warn("Unknown operation:{}", message.getOperation()); + break; + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Integer.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_READ_INTERVAL)) { + return (R) Integer.valueOf(readInterval); + } + return super.onGetAttribute(descriptor); + } + + @Override + public void shutdown() { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + mtk3339gps.shutdown(); + super.shutdown(); + } + + // Private Methods + private synchronized void unregister(RoboReference targetReference) { + List copy = new ArrayList<>(listeners); + for (GPSEventListener listener : copy) { + if (targetReference.equals(listener.target)) { + listeners.remove(listener); + mtk3339gps.removeListener(listener); + // I guess you could theoretically have several registered to + // the same target, so let's keep checking... + } + } + } + + private synchronized void register(RoboReference targetReference) { + GPSEventListener listener = new GPSEventListener(targetReference); + listeners.add(listener); + mtk3339gps.addListener(listener); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java index e527f129..1ef2140f 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gps/TitanGPSUnit.java @@ -16,12 +16,6 @@ */ package com.robo4j.units.rpi.gps; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.Future; -import java.util.concurrent.ScheduledFuture; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; @@ -33,106 +27,114 @@ import com.robo4j.hw.rpi.gps.PositionEvent; import com.robo4j.hw.rpi.gps.VelocityEvent; import com.robo4j.hw.rpi.i2c.gps.TitanX1GPS; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledFuture; /** * Unit for getting GPS data. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class TitanGPSUnit extends RoboUnit { - private final List listeners = new ArrayList<>(); - private TitanX1GPS titangps; - - // The future, if scheduled with the platform scheduler - private volatile ScheduledFuture scheduledFuture; - - private static class GPSEventListener implements GPSListener { - private final RoboReference target; - - private GPSEventListener(RoboReference target) { - this.target = target; - } - - @Override - public void onPosition(PositionEvent event) { - target.sendMessage(event); - } - - @Override - public void onVelocity(VelocityEvent event) { - target.sendMessage(event); - } - } - - public TitanGPSUnit(RoboContext context, String id) { - super(GPSRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - // TODO: Configuration for bus, address etc - try { - titangps = new TitanX1GPS(); - } catch (IOException e) { - throw new ConfigurationException("Could not instantiate GPS!", e); - } - titangps.start(); - } - - @Override - public Future getAttribute(AttributeDescriptor attribute) { - return super.getAttribute(attribute); - } - - @Override - public void onMessage(GPSRequest message) { - super.onMessage(message); - RoboReference targetReference = message.getTarget(); - switch (message.getOperation()) { - case REGISTER: - register(targetReference); - break; - case UNREGISTER: - unregister(targetReference); - break; - default: - SimpleLoggingUtil.error(getClass(), "Unknown operation: " + message.getOperation()); - break; - } - } - - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - return super.onGetAttribute(descriptor); - } - - @Override - public void shutdown() { - if (scheduledFuture != null) { - scheduledFuture.cancel(false); - } - titangps.shutdown(); - super.shutdown(); - } - - // Private Methods - private synchronized void unregister(RoboReference targetReference) { - List copy = new ArrayList<>(listeners); - for (GPSEventListener listener : copy) { - if (targetReference.equals(listener.target)) { - listeners.remove(listener); - titangps.removeListener(listener); - // I guess you could theoretically have several registered to - // the same target, so let's keep checking... - } - } - } - - private synchronized void register(RoboReference targetReference) { - var listener = new GPSEventListener(targetReference); - listeners.add(listener); - titangps.addListener(listener); - } + private static final Logger LOGGER = LoggerFactory.getLogger(TitanGPSUnit.class); + private final List listeners = new ArrayList<>(); + private TitanX1GPS titangps; + + // The future, if scheduled with the platform scheduler + private volatile ScheduledFuture scheduledFuture; + + private static class GPSEventListener implements GPSListener { + private final RoboReference target; + + private GPSEventListener(RoboReference target) { + this.target = target; + } + + @Override + public void onPosition(PositionEvent event) { + target.sendMessage(event); + } + + @Override + public void onVelocity(VelocityEvent event) { + target.sendMessage(event); + } + } + + public TitanGPSUnit(RoboContext context, String id) { + super(GPSRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + // TODO: Configuration for bus, address etc + try { + titangps = new TitanX1GPS(); + } catch (IOException e) { + throw new ConfigurationException("Could not instantiate GPS!", e); + } + titangps.start(); + } + + @Override + public Future getAttribute(AttributeDescriptor attribute) { + return super.getAttribute(attribute); + } + + @Override + public void onMessage(GPSRequest message) { + super.onMessage(message); + RoboReference targetReference = message.getTarget(); + switch (message.getOperation()) { + case REGISTER: + register(targetReference); + break; + case UNREGISTER: + unregister(targetReference); + break; + default: + LOGGER.error("Unknown operation:{}", message.getOperation()); + break; + } + } + + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + return super.onGetAttribute(descriptor); + } + + @Override + public void shutdown() { + if (scheduledFuture != null) { + scheduledFuture.cancel(false); + } + titangps.shutdown(); + super.shutdown(); + } + + // Private Methods + private synchronized void unregister(RoboReference targetReference) { + List copy = new ArrayList<>(listeners); + for (GPSEventListener listener : copy) { + if (targetReference.equals(listener.target)) { + listeners.remove(listener); + titangps.removeListener(listener); + // I guess you could theoretically have several registered to + // the same target, so let's keep checking... + } + } + } + + private synchronized void register(RoboReference targetReference) { + var listener = new GPSEventListener(targetReference); + listeners.add(listener); + titangps.addListener(listener); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java index 35f4923e..352b767f 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/gyro/GyroL3GD20Unit.java @@ -16,15 +16,6 @@ */ package com.robo4j.units.rpi.gyro; -import java.io.IOException; -import java.util.Arrays; -import java.util.Collection; -import java.util.Collections; -import java.util.HashMap; -import java.util.Map; -import java.util.concurrent.ScheduledFuture; -import java.util.concurrent.TimeUnit; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -35,203 +26,211 @@ import com.robo4j.hw.rpi.i2c.gyro.CalibratedGyro; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device; import com.robo4j.hw.rpi.i2c.gyro.GyroL3GD20Device.Sensitivity; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.math.geometry.Tuple3f; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.units.rpi.gyro.GyroRequest.GyroAction; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.Arrays; +import java.util.Collection; +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.ScheduledFuture; +import java.util.concurrent.TimeUnit; /** * Gyro unit based on the {@link GyroL3GD20Device}. Note that there can only be * ONE active notification threshold per target. If a new one is registered * before it has triggered, the new one will replace the old one. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ @WorkTrait public class GyroL3GD20Unit extends I2CRoboUnit { - /** - * This key configures the sensitivity of the gyro. Use the name of the - * Sensitivity enum. Default is DPS_245. - * - * @see Sensitivity - */ - public static final String PROPERTY_KEY_SENSITIVITY = "sensitivity"; - - /** - * This key configures the high pass filter. Set to true to enable. Default - * is true. - * - * @see GyroL3GD20Device - */ - public static final String PROPERTY_KEY_HIGH_PASS_FILTER = "enableHighPass"; - - /** - * This key configures how often to read the gyro in ms. Default is 10. - */ - public static final String PROPERTY_KEY_PERIOD = "period"; - - /** - * This attribute will provide the state of the gyro as a {@link Tuple3f}. - */ - public static final String ATTRIBUTE_NAME_STATE = "state"; - - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE))); - - private final Map, GyroNotificationEntry> activeThresholds = new HashMap<>(); - - private final GyroScanner scanner = new GyroScanner(); - - // TODO review field purpose - private Sensitivity sensitivity; - private boolean highPassFilter; - private int period; - private CalibratedGyro gyro; - private volatile ScheduledFuture readings; - - private class GyroScanner implements Runnable { - private long lastReadingTime = System.currentTimeMillis(); - private Tuple3f lastReading = new Tuple3f(0f, 0f, 0f); - - @Override - public void run() { - Tuple3f data = read(); - long newTime = System.currentTimeMillis(); - - // Trapezoid - Tuple3f tmp = new Tuple3f(data); - long deltaTime = newTime - lastReadingTime; - data.add(lastReading); - data.multiplyScalar(deltaTime / 2000.0f); - - lastReading.set(tmp); - addToDeltas(data); - lastReadingTime = newTime; - } - - private void addToDeltas(Tuple3f data) { - synchronized (GyroL3GD20Unit.this) { - for (GyroNotificationEntry notificationEntry : activeThresholds.values()) { - notificationEntry.addDelta(data); - } - } - } - - private void reset() { - lastReadingTime = System.currentTimeMillis(); - lastReading = read(); - } - - private Tuple3f read() { - try { - return gyro.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read gyro, aborting.", e); - return null; - } - } - } - - /** - * Constructor. - * - * @param context - * the robo context. - * @param id - * the robo unit id. - */ - public GyroL3GD20Unit(RoboContext context, String id) { - super(GyroRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - sensitivity = Sensitivity.valueOf(configuration.getString(PROPERTY_KEY_SENSITIVITY, "DPS_245")); - period = configuration.getInteger(PROPERTY_KEY_PERIOD, 10); - highPassFilter = configuration.getBoolean(PROPERTY_KEY_HIGH_PASS_FILTER, true); - try { - gyro = new CalibratedGyro(new GyroL3GD20Device(getBus(), getAddress(), sensitivity, highPassFilter)); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - @Override - public Collection> getKnownAttributes() { - return KNOWN_ATTRIBUTES; - } - - @Override - public void onMessage(GyroRequest message) { - RoboReference notificationTarget = message.getTarget(); - switch (message.getAction()) { - case CALIBRATE: - try { - gyro.calibrate(); - scanner.reset(); - if (notificationTarget != null) { - notificationTarget.sendMessage(new GyroEvent(new Tuple3f(0, 0, 0))); - } - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to calibrate!", e); - } - break; - case STOP: - stopForNotificationTarget(notificationTarget); - break; - case ONCE: - case CONTINUOUS: - if (message.getNotificationThreshold() != null) { - setUpNotification(message); - } - break; - } - } - - private void stopForNotificationTarget(RoboReference notificationTarget) { - synchronized (this) { - if (notificationTarget == null) { - activeThresholds.clear(); - } else { - activeThresholds.remove(notificationTarget); - } - if (activeThresholds.isEmpty()) { - if (readings != null) { - readings.cancel(false); - readings = null; - } - } - } - } - - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { - try { - return (R) gyro.read(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the gyro!", e); - } - } - return super.onGetAttribute(descriptor); - } - - private void setUpNotification(GyroRequest request) { - synchronized (this) { - if (request.getAction() == GyroAction.CONTINUOUS) { - activeThresholds.put(request.getTarget(), new ContinuousGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); - } else { - activeThresholds.put(request.getTarget(), new FixedGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); - } - } - if (readings == null) { - synchronized (this) { - readings = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); - } - } - } + /** + * This key configures the sensitivity of the gyro. Use the name of the + * Sensitivity enum. Default is DPS_245. + * + * @see Sensitivity + */ + public static final String PROPERTY_KEY_SENSITIVITY = "sensitivity"; + + /** + * This key configures the high pass filter. Set to true to enable. Default + * is true. + * + * @see GyroL3GD20Device + */ + public static final String PROPERTY_KEY_HIGH_PASS_FILTER = "enableHighPass"; + + /** + * This key configures how often to read the gyro in ms. Default is 10. + */ + public static final String PROPERTY_KEY_PERIOD = "period"; + + /** + * This attribute will provide the state of the gyro as a {@link Tuple3f}. + */ + public static final String ATTRIBUTE_NAME_STATE = "state"; + + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(Tuple3f.class, ATTRIBUTE_NAME_STATE))); + private static final Logger LOGGER = LoggerFactory.getLogger(GyroL3GD20Unit.class); + private final Map, GyroNotificationEntry> activeThresholds = new HashMap<>(); + + private final GyroScanner scanner = new GyroScanner(); + + // TODO review field purpose + private Sensitivity sensitivity; + private boolean highPassFilter; + private int period; + private CalibratedGyro gyro; + private volatile ScheduledFuture readings; + + private class GyroScanner implements Runnable { + private long lastReadingTime = System.currentTimeMillis(); + private Tuple3f lastReading = new Tuple3f(0f, 0f, 0f); + + @Override + public void run() { + Tuple3f data = read(); + long newTime = System.currentTimeMillis(); + + // Trapezoid + Tuple3f tmp = new Tuple3f(data); + long deltaTime = newTime - lastReadingTime; + data.add(lastReading); + data.multiplyScalar(deltaTime / 2000.0f); + + lastReading.set(tmp); + addToDeltas(data); + lastReadingTime = newTime; + } + + private void addToDeltas(Tuple3f data) { + synchronized (GyroL3GD20Unit.this) { + for (GyroNotificationEntry notificationEntry : activeThresholds.values()) { + notificationEntry.addDelta(data); + } + } + } + + private void reset() { + lastReadingTime = System.currentTimeMillis(); + lastReading = read(); + } + + private Tuple3f read() { + try { + return gyro.read(); + } catch (IOException e) { + LOGGER.error("Could not read gyro, aborting:{}", e.getMessage(), e); + return null; + } + } + } + + /** + * Constructor. + * + * @param context the robo context. + * @param id the robo unit id. + */ + public GyroL3GD20Unit(RoboContext context, String id) { + super(GyroRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + sensitivity = Sensitivity.valueOf(configuration.getString(PROPERTY_KEY_SENSITIVITY, "DPS_245")); + period = configuration.getInteger(PROPERTY_KEY_PERIOD, 10); + highPassFilter = configuration.getBoolean(PROPERTY_KEY_HIGH_PASS_FILTER, true); + try { + gyro = new CalibratedGyro(new GyroL3GD20Device(getBus(), getAddress(), sensitivity, highPassFilter)); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + @Override + public Collection> getKnownAttributes() { + return KNOWN_ATTRIBUTES; + } + + @Override + public void onMessage(GyroRequest message) { + RoboReference notificationTarget = message.getTarget(); + switch (message.getAction()) { + case CALIBRATE: + try { + gyro.calibrate(); + scanner.reset(); + if (notificationTarget != null) { + notificationTarget.sendMessage(new GyroEvent(new Tuple3f(0, 0, 0))); + } + } catch (IOException e) { + LOGGER.error("Failed to calibrate:{}", e.getMessage(), e); + } + break; + case STOP: + stopForNotificationTarget(notificationTarget); + break; + case ONCE: + case CONTINUOUS: + if (message.getNotificationThreshold() != null) { + setUpNotification(message); + } + break; + } + } + + private void stopForNotificationTarget(RoboReference notificationTarget) { + synchronized (this) { + if (notificationTarget == null) { + activeThresholds.clear(); + } else { + activeThresholds.remove(notificationTarget); + } + if (activeThresholds.isEmpty()) { + if (readings != null) { + readings.cancel(false); + readings = null; + } + } + } + } + + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeType() == Tuple3f.class && descriptor.getAttributeName().equals(ATTRIBUTE_NAME_STATE)) { + try { + return (R) gyro.read(); + } catch (IOException e) { + LOGGER.error("Failed to read the gyro:{}", e.getMessage(), e); + } + } + return super.onGetAttribute(descriptor); + } + + private void setUpNotification(GyroRequest request) { + synchronized (this) { + if (request.getAction() == GyroAction.CONTINUOUS) { + activeThresholds.put(request.getTarget(), new ContinuousGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); + } else { + activeThresholds.put(request.getTarget(), new FixedGyroNotificationEntry(request.getTarget(), request.getNotificationThreshold())); + } + } + if (readings == null) { + synchronized (this) { + readings = getContext().getScheduler().scheduleAtFixedRate(scanner, 0, period, TimeUnit.MILLISECONDS); + } + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java index 00d449fb..d01a9ee3 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/Bno080Unit.java @@ -28,7 +28,8 @@ import com.robo4j.hw.rpi.imu.bno.DataListener; import com.robo4j.hw.rpi.imu.bno.impl.Bno080SPIDevice; import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.ArrayList; @@ -43,91 +44,92 @@ */ public class Bno080Unit extends RoboUnit { - public static final String PROPERTY_REPORT_TYPE = "reportType"; - public static final String PROPERTY_REPORT_DELAY = "reportDelay"; - - private static final class BnoListenerEvent implements DataListener { - private final RoboReference target; - - BnoListenerEvent(RoboReference target) { - this.target = target; - } - - @Override - public void onResponse(DataEvent3f event) { - target.sendMessage(event); - } - } - - private final List listeners = new ArrayList<>(); - private Bno080Device device; - - public Bno080Unit(RoboContext context, String id) { - super(BnoRequest.class, context, id); - } - - // TODO review field purpose - private int reportDelay; - private SensorReportId report; - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - - final String reportType = configuration.getString(PROPERTY_REPORT_TYPE, null); - report = SensorReportId.valueOf(reportType.toUpperCase()); - if (report.equals(SensorReportId.NONE)) { - throw new ConfigurationException(PROPERTY_REPORT_TYPE); - } - - final Integer delay = configuration.getInteger(PROPERTY_REPORT_DELAY, null); - if (delay == null || delay <= 0) { - throw new ConfigurationException(PROPERTY_REPORT_DELAY); - } - this.reportDelay = delay; - - try { - // TODO: make device configurable - device = Bno080Factory.createDefaultSPIDevice(); - } catch (IOException | InterruptedException e) { - throw new ConfigurationException("Could not initiate device", e); - } - device.start(report, reportDelay); - } - - @Override - public void onMessage(BnoRequest message) { - RoboReference target = message.getTarget(); - switch (message.getListenerAction()) { - case REGISTER: - register(target); - break; - case UNREGISTER: - unregister(target); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Unknown operation: %s", message)); - } - - } - - @Override - public void shutdown() { - super.shutdown(); - device.shutdown(); - } - - private synchronized void register(RoboReference target) { - BnoListenerEvent event = new BnoListenerEvent(target); - listeners.add(event); - device.addListener(event); - } - - private synchronized void unregister(RoboReference target) { - for (BnoListenerEvent l : new ArrayList<>(listeners)) { - if (target.equals(l.target)) { - listeners.remove(l); - device.removeListener(l); - } - } - } + public static final String PROPERTY_REPORT_TYPE = "reportType"; + public static final String PROPERTY_REPORT_DELAY = "reportDelay"; + private static final Logger LOGGER = LoggerFactory.getLogger(Bno080Unit.class); + + private static final class BnoListenerEvent implements DataListener { + private final RoboReference target; + + BnoListenerEvent(RoboReference target) { + this.target = target; + } + + @Override + public void onResponse(DataEvent3f event) { + target.sendMessage(event); + } + } + + private final List listeners = new ArrayList<>(); + private Bno080Device device; + + public Bno080Unit(RoboContext context, String id) { + super(BnoRequest.class, context, id); + } + + // TODO review field purpose + private int reportDelay; + private SensorReportId report; + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + + final String reportType = configuration.getString(PROPERTY_REPORT_TYPE, null); + report = SensorReportId.valueOf(reportType.toUpperCase()); + if (report.equals(SensorReportId.NONE)) { + throw new ConfigurationException(PROPERTY_REPORT_TYPE); + } + + final Integer delay = configuration.getInteger(PROPERTY_REPORT_DELAY, null); + if (delay == null || delay <= 0) { + throw new ConfigurationException(PROPERTY_REPORT_DELAY); + } + this.reportDelay = delay; + + try { + // TODO: make device configurable + device = Bno080Factory.createDefaultSPIDevice(); + } catch (IOException | InterruptedException e) { + throw new ConfigurationException("Could not initiate device", e); + } + device.start(report, reportDelay); + } + + @Override + public void onMessage(BnoRequest message) { + RoboReference target = message.getTarget(); + switch (message.getListenerAction()) { + case REGISTER: + register(target); + break; + case UNREGISTER: + unregister(target); + break; + default: + LOGGER.error("Unknown operation: {}", message); + } + + } + + @Override + public void shutdown() { + super.shutdown(); + device.shutdown(); + } + + private synchronized void register(RoboReference target) { + BnoListenerEvent event = new BnoListenerEvent(target); + listeners.add(event); + device.addListener(event); + } + + private synchronized void unregister(RoboReference target) { + for (BnoListenerEvent l : new ArrayList<>(listeners)) { + if (target.equals(l.target)) { + listeners.remove(l); + device.removeListener(l); + } + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java index 4389cfbf..4c6cd526 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/DataEventListenerUnit.java @@ -20,7 +20,8 @@ import com.robo4j.RoboContext; import com.robo4j.RoboUnit; import com.robo4j.hw.rpi.imu.bno.DataEvent3f; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * DataEventListener unit listens to {@link DataEvent3f} event types produced by @@ -30,13 +31,14 @@ * @author Miroslav Wengner (@miragemiko) */ public class DataEventListenerUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(DataEventListenerUnit.class); - public DataEventListenerUnit(RoboContext context, String id) { - super(DataEvent3f.class, context, id); - } + public DataEventListenerUnit(RoboContext context, String id) { + super(DataEvent3f.class, context, id); + } - @Override - public void onMessage(DataEvent3f message) { - SimpleLoggingUtil.info(getClass(), "received:" + message); - } + @Override + public void onMessage(DataEvent3f message) { + LOGGER.info("received:{}", message); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java index 1fde88a2..a0505f83 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/imu/VectorEventListenerUnit.java @@ -23,8 +23,9 @@ import com.robo4j.RoboUnit; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.imu.bno.VectorEvent; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.net.LookupServiceProvider; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * BNOVectorEventListenerUnit listen to VectorEvent events produced by {@link Bno080Unit} @@ -33,34 +34,34 @@ * @author Miroslav Wengner (@miragemiko) */ public class VectorEventListenerUnit extends RoboUnit { - - public static final String ATTR_TARGET_CONTEXT = "targetContext"; - public static final String ATTR_REMOTE_UNIT = "remoteUnit"; + public static final String ATTR_TARGET_CONTEXT = "targetContext"; + public static final String ATTR_REMOTE_UNIT = "remoteUnit"; + private static final Logger LOGGER = LoggerFactory.getLogger(VectorEventListenerUnit.class); - public VectorEventListenerUnit(RoboContext context, String id) { - super(VectorEvent.class, context, id); - } + public VectorEventListenerUnit(RoboContext context, String id) { + super(VectorEvent.class, context, id); + } - private String targetContext; - private String remoteUnit; + private String targetContext; + private String remoteUnit; - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - targetContext = configuration.getString(ATTR_TARGET_CONTEXT, null); - remoteUnit = configuration.getString(ATTR_REMOTE_UNIT, null); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + targetContext = configuration.getString(ATTR_TARGET_CONTEXT, null); + remoteUnit = configuration.getString(ATTR_REMOTE_UNIT, null); + } - @Override - public void onMessage(VectorEvent message) { - SimpleLoggingUtil.info(getClass(), "received:" + message); - RoboContext remoteContext = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); - if(remoteContext != null){ - RoboReference roboReference = remoteContext.getReference(remoteUnit); - if(roboReference != null){ - roboReference.sendMessage(message); - } - } else { - SimpleLoggingUtil.info(getClass(), String.format("context not found: %s", targetContext)); - } - } + @Override + public void onMessage(VectorEvent message) { + LOGGER.info("received:{}", message); + RoboContext remoteContext = LookupServiceProvider.getDefaultLookupService().getContext(targetContext); + if (remoteContext != null) { + RoboReference roboReference = remoteContext.getReference(remoteUnit); + if (roboReference != null) { + roboReference.sendMessage(message); + } + } else { + LOGGER.warn("context not found: {}", targetContext); + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java index c3b8f0d1..47d75dcf 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitButtonUnit.java @@ -16,8 +16,6 @@ */ package com.robo4j.units.rpi.lcd; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.LifecycleState; import com.robo4j.RoboContext; @@ -28,101 +26,105 @@ import com.robo4j.hw.rpi.i2c.adafruitlcd.Button; import com.robo4j.hw.rpi.i2c.adafruitlcd.ButtonListener; import com.robo4j.hw.rpi.i2c.adafruitlcd.ButtonPressedObserver; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * A {@link RoboUnit} for the button part of the Adafruit 16x2 character LCD * shield. Having a separate unit allows the buttons to be used independently of * the LCD. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitButtonUnit extends I2CRoboUnit { - private AdafruitLcd lcd; - private ButtonPressedObserver observer; - private String target; - private ButtonListener buttonListener; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitButtonUnit.class); + private AdafruitLcd lcd; + private ButtonPressedObserver observer; + private String target; + private ButtonListener buttonListener; - public AdafruitButtonUnit(RoboContext context, String id) { - super(Object.class, context, id); - } + public AdafruitButtonUnit(RoboContext context, String id) { + super(Object.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - target = configuration.getString("target", null); - if (target == null) { - throw ConfigurationException.createMissingConfigNameException("target"); - } - try { - lcd = AdafruitLcdUnit.getLCD(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException("Could not initialize LCD shield", e); - } - setState(LifecycleState.INITIALIZED); - } + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + target = configuration.getString("target", null); + if (target == null) { + throw ConfigurationException.createMissingConfigNameException("target"); + } + try { + lcd = AdafruitLcdUnit.getLCD(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException("Could not initialize LCD shield", e); + } + setState(LifecycleState.INITIALIZED); + } - @Override - public void start() { - final RoboReference targetRef = getContext().getReference(target); - setState(LifecycleState.STARTING); - observer = new ButtonPressedObserver(lcd); - buttonListener = (Button button) -> { - if (getState() == LifecycleState.STARTED) { - try { - switch (button) { - case UP: - targetRef.sendMessage(AdafruitButtonEnum.UP); - break; - case DOWN: - targetRef.sendMessage(AdafruitButtonEnum.DOWN); - break; - case RIGHT: - targetRef.sendMessage(AdafruitButtonEnum.LEFT); - break; - case LEFT: - targetRef.sendMessage(AdafruitButtonEnum.RIGHT); - break; - case SELECT: - targetRef.sendMessage(AdafruitButtonEnum.SELECT); - break; - default: - lcd.clear(); - lcd.setText(String.format("Button %s\nis not in use...", button.toString())); - } - } catch (IOException e) { - handleException(e); - } - } - }; + @Override + public void start() { + final RoboReference targetRef = getContext().getReference(target); + setState(LifecycleState.STARTING); + observer = new ButtonPressedObserver(lcd); + buttonListener = (Button button) -> { + if (getState() == LifecycleState.STARTED) { + try { + switch (button) { + case UP: + targetRef.sendMessage(AdafruitButtonEnum.UP); + break; + case DOWN: + targetRef.sendMessage(AdafruitButtonEnum.DOWN); + break; + case RIGHT: + targetRef.sendMessage(AdafruitButtonEnum.LEFT); + break; + case LEFT: + targetRef.sendMessage(AdafruitButtonEnum.RIGHT); + break; + case SELECT: + targetRef.sendMessage(AdafruitButtonEnum.SELECT); + break; + default: + lcd.clear(); + lcd.setText(String.format("Button %s\nis not in use...", button.toString())); + } + } catch (IOException e) { + handleException(e); + } + } + }; - observer.addButtonListener(buttonListener); - setState(LifecycleState.STARTED); - } + observer.addButtonListener(buttonListener); + setState(LifecycleState.STARTED); + } - @Override - public void stop() { - observer.removeButtonListener(buttonListener); - observer = null; - buttonListener = null; - } + @Override + public void stop() { + observer.removeButtonListener(buttonListener); + observer = null; + buttonListener = null; + } - @Override - public void shutdown() { - try { - lcd.stop(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to disconnect LCD", e); - } - } + @Override + public void shutdown() { + try { + lcd.stop(); + } catch (IOException e) { + LOGGER.error("Failed to disconnect LCD:{}", e.getMessage(), e); + } + } - //Private Methods - private void handleException(IOException e) { - setState(LifecycleState.STOPPING); - shutdown(); - setState(LifecycleState.FAILED); - } + //Private Methods + private void handleException(IOException e) { + setState(LifecycleState.STOPPING); + shutdown(); + setState(LifecycleState.FAILED); + } } \ No newline at end of file diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java index bc64a605..983339a1 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lcd/AdafruitLcdUnit.java @@ -28,11 +28,12 @@ import com.robo4j.hw.rpi.i2c.adafruitlcd.LcdFactory; import com.robo4j.hw.rpi.i2c.adafruitlcd.impl.AdafruitLcdImpl.Direction; import com.robo4j.hw.rpi.utils.I2cBus; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CEndPoint; import com.robo4j.units.rpi.I2CRegistry; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.util.StringConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.io.IOException; import java.util.Arrays; @@ -42,176 +43,164 @@ /** * A {@link RoboUnit} for the Adafruit 16x2 character LCD shield. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitLcdUnit extends I2CRoboUnit { - private static final String ATTRIBUTE_NAME_COLOR = "color"; - private static final String ATTRIBUTE_NAME_TEXT = "text"; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitLcdUnit.class); + private static final String ATTRIBUTE_NAME_COLOR = "color"; + private static final String ATTRIBUTE_NAME_TEXT = "text"; - public static final Collection> KNOWN_ATTRIBUTES = Collections - .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(String.class, ATTRIBUTE_NAME_TEXT), - DefaultAttributeDescriptor.create(Color.class, ATTRIBUTE_NAME_COLOR))); + public static final Collection> KNOWN_ATTRIBUTES = Collections + .unmodifiableCollection(Arrays.asList(DefaultAttributeDescriptor.create(String.class, ATTRIBUTE_NAME_TEXT), + DefaultAttributeDescriptor.create(Color.class, ATTRIBUTE_NAME_COLOR))); - private final AtomicReference stringMessage = new AtomicReference<>(StringConstants.EMPTY); - private AdafruitLcd lcd; + private final AtomicReference stringMessage = new AtomicReference<>(StringConstants.EMPTY); + private AdafruitLcd lcd; - public AdafruitLcdUnit(RoboContext context, String id) { - super(LcdMessage.class, context, id); - } + public AdafruitLcdUnit(RoboContext context, String id) { + super(LcdMessage.class, context, id); + } - /** - * - * @param bus - * used bus - * @param address - * desired address + /** + * @param bus used bus + * @param address desired address */ - static AdafruitLcd getLCD(I2cBus bus, int address) throws IOException { - Object lcd = I2CRegistry.getI2CDeviceByEndPoint(new I2CEndPoint(bus, address)); - if (lcd == null) { - try { - lcd = LcdFactory.createLCD(bus, address); - // Note that we cannot catch hardware specific exceptions here, - // since they will be loaded when we run as mocked. - } catch (Exception e) { - throw new IOException(e); - } - I2CRegistry.registerI2CDevice(lcd, new I2CEndPoint(bus, address)); - } - return (AdafruitLcd) lcd; - } + static AdafruitLcd getLCD(I2cBus bus, int address) throws IOException { + Object lcd = I2CRegistry.getI2CDeviceByEndPoint(new I2CEndPoint(bus, address)); + if (lcd == null) { + try { + lcd = LcdFactory.createLCD(bus, address); + // Note that we cannot catch hardware specific exceptions here, + // since they will be loaded when we run as mocked. + } catch (Exception e) { + throw new IOException(e); + } + I2CRegistry.registerI2CDevice(lcd, new I2CEndPoint(bus, address)); + } + return (AdafruitLcd) lcd; + } - /** - * - * @param message - * the message received by this unit. - * - */ - @Override - public void onMessage(LcdMessage message) { - try { - processLcdMessage(message); - } catch (Exception e) { - SimpleLoggingUtil.debug(getClass(), "Could not accept message" + message.toString(), e); - } - } + /** + * @param message the message received by this unit. + */ + @Override + public void onMessage(LcdMessage message) { + try { + processLcdMessage(message); + } catch (Exception e) { + LOGGER.error("Could not accept message:{}", message, e); + } + } - /** - * - * @param configuration - * - unit configuration - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - try { - lcd = getLCD(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException("Could not initialize LCD", e); - } - } + /** + * @param configuration - unit configuration + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + try { + lcd = getLCD(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException("Could not initialize LCD", e); + } + } - @Override - public void stop() { - setState(LifecycleState.STOPPING); - try { - lcd.clear(); - lcd.setDisplayEnabled(false); - lcd.stop(); - } catch (IOException e) { - throw new AdafruitException("Could not disconnect LCD", e); - } - setState(LifecycleState.STOPPED); - } + @Override + public void stop() { + setState(LifecycleState.STOPPING); + try { + lcd.clear(); + lcd.setDisplayEnabled(false); + lcd.stop(); + } catch (IOException e) { + throw new AdafruitException("Could not disconnect LCD", e); + } + setState(LifecycleState.STOPPED); + } - /** - * @param message - * accepted message type - * @throws IOException - */ - private void processLcdMessage(LcdMessage message) throws IOException { - switch (message.getType()) { - case CLEAR: - lcd.clear(); - break; - case DISPLAY_ENABLE: - final boolean disen = Boolean.valueOf(message.getText().trim()); - lcd.setDisplayEnabled(disen); - break; - case SCROLL: - // TODO: consider enum as the constant - switch (message.getText().trim()) { - case "left": - lcd.scrollDisplay(Direction.LEFT); - break; - case "right": - lcd.scrollDisplay(Direction.RIGHT); - break; - default: - SimpleLoggingUtil.error(getClass(), "Scroll direction " + message.getText() + " is unknown"); - break; - } - break; - case SET_TEXT: - if (message.getColor() != null) { - lcd.setBacklight(message.getColor()); - } - if (message.getText() != null) { - String text = message.getText(); - lcd.setText(text); - stringMessage.set(text); - } - break; - case STOP: - lcd.stop(); - break; - default: - SimpleLoggingUtil.error(getClass(), message.getType() + "demo not supported!"); - break; - } - } + /** + * @param message accepted message type + * @throws IOException + */ + private void processLcdMessage(LcdMessage message) throws IOException { + switch (message.getType()) { + case CLEAR: + lcd.clear(); + break; + case DISPLAY_ENABLE: + final boolean disen = Boolean.valueOf(message.getText().trim()); + lcd.setDisplayEnabled(disen); + break; + case SCROLL: + // TODO: consider enum as the constant + switch (message.getText().trim()) { + case "left": + lcd.scrollDisplay(Direction.LEFT); + break; + case "right": + lcd.scrollDisplay(Direction.RIGHT); + break; + default: + LOGGER.warn("unknown scroll direction:{}", message.getText()); + break; + } + break; + case SET_TEXT: + if (message.getColor() != null) { + lcd.setBacklight(message.getColor()); + } + if (message.getText() != null) { + String text = message.getText(); + lcd.setText(text); + stringMessage.set(text); + } + break; + case STOP: + lcd.stop(); + break; + default: + LOGGER.warn("demo not supported:{}", message.getType()); + break; + } + } - @SuppressWarnings("unchecked") - @Override - public R onGetAttribute(AttributeDescriptor attribute) { - if (ATTRIBUTE_NAME_TEXT.equals(attribute.getAttributeName())) { - return (R) stringMessage.get(); - } else if (ATTRIBUTE_NAME_COLOR.equals(attribute.getAttributeName())) { - try { - return (R) lcd.getBacklight(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read the color", e); - } - } - return null; - } + @SuppressWarnings("unchecked") + @Override + public R onGetAttribute(AttributeDescriptor attribute) { + if (ATTRIBUTE_NAME_TEXT.equals(attribute.getAttributeName())) { + return (R) stringMessage.get(); + } else if (ATTRIBUTE_NAME_COLOR.equals(attribute.getAttributeName())) { + try { + return (R) lcd.getBacklight(); + } catch (IOException e) { + LOGGER.error("Failed to read the color:{}", e.getMessage(), e); + } + } + return null; + } - @Override - public Collection> getKnownAttributes() { - return KNOWN_ATTRIBUTES; - } + @Override + public Collection> getKnownAttributes() { + return KNOWN_ATTRIBUTES; + } - /** - * Fill one of the first 8 CGRAM locations with custom characters. The location - * parameter should be between 0 and 7 and pattern should provide an array of 8 - * bytes containing the pattern. e.g. you can design your custom character at - * <a - * href=http://www.quinapalus.com/hd44780udg.html> http://www.quinapalus.com/hd44780udg.html<a/> . - * To show your custom character obtain the string representation for the - * location e.g. String.format("custom char=%c", 0). - * - * @param location - * storage location for this character, between 0 and 7 - * @param pattern - * array of 8 bytes containing the character's pattern - * @throws IOException - * exception - */ - public void createChar(final int location, final byte[] pattern) throws IOException { - lcd.createChar(location, pattern); - } + /** + * Fill one of the first 8 CGRAM locations with custom characters. The location + * parameter should be between 0 and 7 and pattern should provide an array of 8 + * bytes containing the pattern. e.g. you can design your custom character at + * <a + * href=http://www.quinapalus.com/hd44780udg.html> http://www.quinapalus.com/hd44780udg.html<a/> . + * To show your custom character obtain the string representation for the + * location e.g. String.format("custom char=%c", 0). + * + * @param location storage location for this character, between 0 and 7 + * @param pattern array of 8 bytes containing the character's pattern + * @throws IOException exception + */ + public void createChar(final int location, final byte[] pattern) throws IOException { + lcd.createChar(location, pattern); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java index de075033..f287c32c 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AbstractI2CBackpackUnit.java @@ -19,44 +19,42 @@ import com.robo4j.RoboContext; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AbstractBackpack; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * AbstractI2CBackpackUnit abstract I2C LedBackpack unit * - * @param - * backpack unit type - * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ abstract class AbstractI2CBackpackUnit extends I2CRoboUnit { + static final String ATTRIBUTE_ADDRESS = "address"; + static final String ATTRIBUTE_BUS = "bus"; + static final String ATTRIBUTE_BRIGHTNESS = "brightness"; + private static final Logger LOGGER = LoggerFactory.getLogger(AbstractI2CBackpackUnit.class); - static final String ATTRIBUTE_ADDRESS = "address"; - static final String ATTRIBUTE_BUS = "bus"; - static final String ATTRIBUTE_BRIGHTNESS = "brightness"; - - AbstractI2CBackpackUnit(Class messageType, RoboContext context, String id) { - super(messageType, context, id); - } + AbstractI2CBackpackUnit(Class messageType, RoboContext context, String id) { + super(messageType, context, id); + } - void processMessage(AbstractBackpack device, DrawMessage message) { - switch (message.getType()) { - case CLEAR: - device.clear(); - break; - case PAINT: - paint(message); - break; - case DISPLAY: - paint(message); - device.display(); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Illegal message: %s", message)); - } - } + void processMessage(AbstractBackpack device, DrawMessage message) { + switch (message.getType()) { + case CLEAR: + device.clear(); + break; + case PAINT: + paint(message); + break; + case DISPLAY: + paint(message); + device.display(); + break; + default: + LOGGER.warn("Illegal message: {}", message); + } + } - abstract void paint(DrawMessage message); + abstract void paint(DrawMessage message); } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java index 487bcb40..76ba7cf0 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/led/AdafruitAlphanumericUnit.java @@ -17,19 +17,20 @@ package com.robo4j.units.rpi.led; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AbstractBackpack; import com.robo4j.hw.rpi.i2c.adafruitbackpack.AlphanumericDevice; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * AdafruitAlphanumericUnit - * + *

* This version of the LED backpack is designed for two dual 14-segment * "Alphanumeric" displays. These 14-segment displays normally require 18 pins * (4 'characters' and 14 total segments each) This backpack solves the @@ -38,60 +39,61 @@ * controller chip takes care of everything, drawing all the LEDs in the * background. All you have to do is write data to it using the 2-pin I2C * interface - * + *

* see https://learn.adafruit.com/adafruit-led-backpack/0-54-alphanumeric * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class AdafruitAlphanumericUnit extends I2CRoboUnit { - private AlphanumericDevice device; + private static final Logger LOGGER = LoggerFactory.getLogger(AdafruitAlphanumericUnit.class); + private AlphanumericDevice device; - public AdafruitAlphanumericUnit(RoboContext context, String id) { - super(AlphaNumericMessage.class, context, id); - } + public AdafruitAlphanumericUnit(RoboContext context, String id) { + super(AlphaNumericMessage.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - int brightness = configuration.getInteger(AbstractI2CBackpackUnit.ATTRIBUTE_BRIGHTNESS, AbstractBackpack.DEFAULT_BRIGHTNESS); + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + int brightness = configuration.getInteger(AbstractI2CBackpackUnit.ATTRIBUTE_BRIGHTNESS, AbstractBackpack.DEFAULT_BRIGHTNESS); - try { - device = new AlphanumericDevice(getBus(), getAddress(), brightness); - } catch (IOException e) { - throw new ConfigurationException("Failed to instantiate device", e); - } - } + try { + device = new AlphanumericDevice(getBus(), getAddress(), brightness); + } catch (IOException e) { + throw new ConfigurationException("Failed to instantiate device", e); + } + } - @Override - public void onMessage(AlphaNumericMessage message) { - switch (message.getCommand()) { - case CLEAR: - device.clear(); - break; - case PAINT: - render(message); - break; - case DISPLAY: - render(message); - device.display(); - break; - default: - SimpleLoggingUtil.error(getClass(), String.format("Illegal message: %s", message)); - } - } + @Override + public void onMessage(AlphaNumericMessage message) { + switch (message.getCommand()) { + case CLEAR: + device.clear(); + break; + case PAINT: + render(message); + break; + case DISPLAY: + render(message); + device.display(); + break; + default: + LOGGER.error("Illegal message: {}", message); + } + } - private void render(AlphaNumericMessage message) { - if (message.getStartPosition() == -1) { - for (int i = 0; i < message.getCharacters().length; i++) { - device.addCharacter((char) message.getCharacters()[i], message.getDots()[i]); - } - } else { - for (int i = 0; i < message.getCharacters().length; i++) { - device.setCharacter((message.getStartPosition() + i) % device.getNumberOfCharacters(), (char) message.getCharacters()[i], - message.getDots()[i]); - } - } + private void render(AlphaNumericMessage message) { + if (message.getStartPosition() == -1) { + for (int i = 0; i < message.getCharacters().length; i++) { + device.addCharacter((char) message.getCharacters()[i], message.getDots()[i]); + } + } else { + for (int i = 0; i < message.getCharacters().length; i++) { + device.setCharacter((message.getStartPosition() + i) % device.getNumberOfCharacters(), (char) message.getCharacters()[i], + message.getDots()[i]); + } + } - } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java index ba7cc4b9..3918911b 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/lidarlite/LaserScanner.java @@ -16,18 +16,11 @@ */ package com.robo4j.units.rpi.lidarlite; -import java.io.IOException; -import java.util.concurrent.ExecutionException; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.function.Predicate; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.RoboReference; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.lidar.LidarLiteDevice; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.math.geometry.Point2f; import com.robo4j.math.geometry.ScanResult2D; import com.robo4j.math.geometry.impl.ScanResultImpl; @@ -35,325 +28,331 @@ import com.robo4j.math.jfr.ScanEvent; import com.robo4j.units.rpi.I2CRoboUnit; import com.robo4j.units.rpi.pwm.PCA9685ServoUnit; -import jdk.jfr.Event; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.function.Predicate; /** * This unit controls a servo to do laser range sweep. - * + *

* <p> Configuration: </p> <li> <ul> pan: the servo to * use for panning the laser. Defaults to "laserscanner.servo". Set to "null" to * not use a servo for panning. </ul> <ul> servoRange: the range in * degrees that send 1.0 (full) to a servo will result in. This must be measured * by testing your hardware. Changing configuration on your servo will change * this too. </ul> </li> - * + *

* FIXME(Marcus/Mar 10, 2017): Only supports the pan servo for now. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class LaserScanner extends I2CRoboUnit { - /** - * Since the lidar lite will sometimes give some abnormal readings with very - * short range, we need to be able to filter out readings. Set this - * depending on your hardware and needs. - */ - private final static float DEFAULT_FILTER_MIN_RANGE = 0.08f; - private Predicate pointFilter; - private String pan; - private LidarLiteDevice lidar; - private float servoRange; - private float angularSpeed; - private float minimumAcquisitionTime; - private float trim; - - /** - * Filter for filtering out mis-reads. Anything closer than minRange will be - * filtered out. - */ - private static class MinRangePointFilter implements Predicate { - private final float minRange; - - private MinRangePointFilter(float minRange) { - this.minRange = minRange; - } - - @Override - public boolean test(Point2f t) { - return t.getRange() >= minRange; - } - } - - private final static class ScanJob implements Runnable { - private final AtomicInteger invokeCount = new AtomicInteger(0); - private final ScanResultImpl scanResult; - private final ScanRequest request; - private final RoboReference recipient; - private final RoboReference servo; - private final boolean lowToHigh; - private final int numberOfScans; - private long delayMicros; - private final float trim; - private final float servoRange; - private final LidarLiteDevice lidar; - private volatile float currentAngle; - private volatile boolean finished = false; - private final ScanEvent scanEvent; - - /** - * - * @param lowToHigh - * @param minimumServoMovementTime - * the minimum time for the servo to complete the movement - * over the range, in seconds - * @param minimumAcquisitionTime - * @param trim - * @param request - * @param servo - * @param servoRange - * @param lidar - * @param recipient - */ - public ScanJob(boolean lowToHigh, float minimumServoMovementTime, float minimumAcquisitionTime, float trim, ScanRequest request, - RoboReference servo, float servoRange, LidarLiteDevice lidar, RoboReference recipient, - Predicate pointFilter) { - this.lowToHigh = lowToHigh; - this.trim = trim; - this.request = request; - this.servo = servo; - this.servoRange = servoRange; - this.lidar = lidar; - this.recipient = recipient; - // one move, one first acquisition - this.numberOfScans = calculateNumberOfScans() + 1; - this.delayMicros = calculateDelay(minimumAcquisitionTime, minimumServoMovementTime); - this.currentAngle = lowToHigh ? request.getStartAngle() : request.getStartAngle() + request.getRange(); - this.scanResult = new ScanResultImpl(100, request.getStep(), pointFilter); - scanEvent = new ScanEvent(scanResult.getScanID(), getScanInfo()); - scanEvent.setScanLeftRight(lowToHigh); - JfrUtils.begin(scanEvent); - } - - @Override - public void run() { - int currentRun = invokeCount.incrementAndGet(); - if (currentRun == 1) { - // On first step, only move servo to start position - float normalizedServoTarget = getNormalizedAngle(); - servo.sendMessage(normalizedServoTarget); - return; - } else if (currentRun == 2) { - // On second, just start acquisition (no point to read yet) - startAcquisition(); - } else if (currentRun > numberOfScans) { - doScan(); - finish(); - } else { - doScan(); - } - // FIXME(Marcus/Mar 10, 2017): May want to synchronize this... - updateTargetAngle(); - } - - private void startAcquisition() { - try { - lidar.acquireRange(); - servo.sendMessage(getNormalizedAngle()); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read laser!", e); - } - } - - private float getNormalizedAngle() { - return this.currentAngle / this.servoRange; - } - - private void doScan() { - // Read previous acquisition - try { - float readDistance = lidar.readDistance(); - // Laser was actually shooting at the previous angle, before - // moving - recalculate for that angle - float lastAngle = currentAngle + (lowToHigh ? -request.getStep() - trim : request.getStep() + trim); - scanResult.addPoint(Point2f.fromPolar(readDistance, (float) Math.toRadians(lastAngle))); - servo.sendMessage(getNormalizedAngle()); - lidar.acquireRange(); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not read laser!", e); - } - } - - private void finish() { - if (!finished) { - recipient.sendMessage(scanResult); - finished = true; - JfrUtils.end(scanEvent); - JfrUtils.commit(scanEvent); - } else { - SimpleLoggingUtil.error(getClass(), "Tried to scan more laser points after being finished!"); - } - } - - private void updateTargetAngle() { - if (lowToHigh) { - currentAngle += request.getStep(); - } else { - currentAngle -= request.getStep(); - } - } - - private int calculateNumberOfScans() { - return Math.round(request.getRange() / request.getStep()); - } - - // FIXME(Marcus/Mar 10, 2017): Calculate the required delay later from - // physical model. - private long calculateDelay(float minimumAcquisitionTime, float minimumServoMovementTime) { - float delayPerStep = minimumServoMovementTime * 1000 / calculateNumberOfScans(); - // If we have a slow servo, we will need to wait for the servo to - // move. If we have a slow acquisition, we will need to wait for the - // laser before continuing - float actualDelay = Math.max(delayPerStep, minimumAcquisitionTime); - return Math.round(actualDelay * 1000.0d); - } - - private String getScanInfo() { - return String.format("start: %2.1f, end: %2.1f, step:%2.1f", request.getStartAngle(), - request.getStartAngle() + request.getRange(), request.getStep()); - } - } - - private final static class ScanFixedAngleJob implements Runnable { - private final RoboReference recipient; - private final LidarLiteDevice lidar; - private final float angle; - private final Predicate filter; - private volatile boolean firstRun = true; - - public ScanFixedAngleJob(RoboReference recipient, LidarLiteDevice lidar, float angle, Predicate filter) { - this.recipient = recipient; - this.lidar = lidar; - this.angle = angle; - this.filter = filter; - } - - @Override - public void run() { - try { - if (firstRun) { - firstRun = false; - lidar.acquireRange(); - } else { - ScanResultImpl scan = new ScanResultImpl(1, 0f, filter); - scan.addPoint(lidar.readDistance(), (float) Math.toRadians(angle)); - recipient.sendMessage(scan); - } - } catch (IOException e) { - SimpleLoggingUtil.debug(getClass(), "Failed to read lidar", e); - } - } - } - - public LaserScanner(RoboContext context, String id) { - super(ScanRequest.class, context, id); - } - - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - pan = configuration.getString("servo", "laserscanner.servo"); - - // Using degrees for convenience - servoRange = (float) configuration.getFloat("servoRange", 45.0f); - - // Using angular degrees per second. - angularSpeed = configuration.getFloat("angularSpeed", 90.0f); - - // Minimum acquisition time, in ms - minimumAcquisitionTime = configuration.getFloat("minAquisitionTime", 2.0f); - - // Trim to align left to right and right to left scans (in degrees) - trim = configuration.getFloat("trim", 0.0f); - - // Set the default minimal range to filter out - float minFilterRange = configuration.getFloat("minFilterRange", DEFAULT_FILTER_MIN_RANGE); - pointFilter = new MinRangePointFilter(minFilterRange); - - try { - lidar = new LidarLiteDevice(getBus(), getAddress()); - } catch (IOException e) { - throw new ConfigurationException(String.format( - "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); - } - } - - @Override - public void onMessage(ScanRequest message) { - RoboReference servo = getPanServo(); - if (message.getRange() == 0 || message.getStep() == 0) { - scheduleFixPanScan(message, servo); - } else { - scheduleScan(message, servo); - } - } - - private void scheduleFixPanScan(ScanRequest message, RoboReference servo) { - servo.sendMessage(message.getStartAngle() / servoRange); - ScanFixedAngleJob fixedAngleJob = new ScanFixedAngleJob(message.getReceiver(), lidar, message.getStartAngle(), pointFilter); - // FIXME(Marcus/Sep 5, 2017): We should try to calculate this - we are - // now assuming that it will take little time to move the servo to the - // "new" position, however, the fact is that the new position will - // usually be the old position. - getContext().getScheduler().schedule(fixedAngleJob, 31, TimeUnit.MILLISECONDS); - getContext().getScheduler().schedule(fixedAngleJob, (long) (31 + minimumAcquisitionTime), TimeUnit.MILLISECONDS); - } - - private RoboReference getPanServo() { - RoboReference servo = getReference(pan); - return servo; - } - - private void scheduleScan(ScanRequest message, RoboReference servo) { - float currentInput = getCurrentInput(servo); - float midPoint = message.getStartAngle() + message.getRange() / 2; - boolean lowToHigh = false; - if (inputToAngle(currentInput) <= midPoint) { - lowToHigh = true; - } - float minimumServoMovementTime = message.getRange() / angularSpeed; - ScanJob job = new ScanJob(lowToHigh, minimumServoMovementTime, minimumAcquisitionTime, trim, message, servo, servoRange, lidar, - message.getReceiver(), pointFilter); - schedule(job); - } - - private float inputToAngle(float currentInput) { - return servoRange * currentInput; - } - - private void schedule(ScanJob job) { - long actualDelayMicros = job.delayMicros; - // One extra for first servo move. - for (int i = 0; i < job.numberOfScans + 1; i++) { - // FIXME(Marcus/Apr 4, 2017): Simplified - need to take angular - // speed of the servo into account. - getContext().getScheduler().schedule(job, actualDelayMicros, TimeUnit.MICROSECONDS); - actualDelayMicros += job.delayMicros; - } - } - - private float getCurrentInput(RoboReference servo) { - try { - return servo.getAttribute(PCA9685ServoUnit.ATTRIBUTE_SERVO_INPUT).get(); - } catch (InterruptedException | ExecutionException e) { - SimpleLoggingUtil.error(getClass(), "Could not read servo input!", e); - return 0; - } - } - - private RoboReference getReference(String unit) { - if (unit.equals("null")) { - return null; - } - return getContext().getReference(unit); - } + private static final Logger LOGGER = LoggerFactory.getLogger(LaserScanner.class); + /** + * Since the lidar lite will sometimes give some abnormal readings with very + * short range, we need to be able to filter out readings. Set this + * depending on your hardware and needs. + */ + private final static float DEFAULT_FILTER_MIN_RANGE = 0.08f; + private Predicate pointFilter; + private String pan; + private LidarLiteDevice lidar; + private float servoRange; + private float angularSpeed; + private float minimumAcquisitionTime; + private float trim; + + /** + * Filter for filtering out mis-reads. Anything closer than minRange will be + * filtered out. + */ + private static class MinRangePointFilter implements Predicate { + private final float minRange; + + private MinRangePointFilter(float minRange) { + this.minRange = minRange; + } + + @Override + public boolean test(Point2f t) { + return t.getRange() >= minRange; + } + } + + private final static class ScanJob implements Runnable { + private final AtomicInteger invokeCount = new AtomicInteger(0); + private final ScanResultImpl scanResult; + private final ScanRequest request; + private final RoboReference recipient; + private final RoboReference servo; + private final boolean lowToHigh; + private final int numberOfScans; + private long delayMicros; + private final float trim; + private final float servoRange; + private final LidarLiteDevice lidar; + private volatile float currentAngle; + private volatile boolean finished = false; + private final ScanEvent scanEvent; + + /** + * @param lowToHigh + * @param minimumServoMovementTime the minimum time for the servo to complete the movement + * over the range, in seconds + * @param minimumAcquisitionTime + * @param trim + * @param request + * @param servo + * @param servoRange + * @param lidar + * @param recipient + */ + public ScanJob(boolean lowToHigh, float minimumServoMovementTime, float minimumAcquisitionTime, float trim, ScanRequest request, + RoboReference servo, float servoRange, LidarLiteDevice lidar, RoboReference recipient, + Predicate pointFilter) { + this.lowToHigh = lowToHigh; + this.trim = trim; + this.request = request; + this.servo = servo; + this.servoRange = servoRange; + this.lidar = lidar; + this.recipient = recipient; + // one move, one first acquisition + this.numberOfScans = calculateNumberOfScans() + 1; + this.delayMicros = calculateDelay(minimumAcquisitionTime, minimumServoMovementTime); + this.currentAngle = lowToHigh ? request.getStartAngle() : request.getStartAngle() + request.getRange(); + this.scanResult = new ScanResultImpl(100, request.getStep(), pointFilter); + scanEvent = new ScanEvent(scanResult.getScanID(), getScanInfo()); + scanEvent.setScanLeftRight(lowToHigh); + JfrUtils.begin(scanEvent); + } + + @Override + public void run() { + int currentRun = invokeCount.incrementAndGet(); + if (currentRun == 1) { + // On first step, only move servo to start position + float normalizedServoTarget = getNormalizedAngle(); + servo.sendMessage(normalizedServoTarget); + return; + } else if (currentRun == 2) { + // On second, just start acquisition (no point to read yet) + startAcquisition(); + } else if (currentRun > numberOfScans) { + doScan(); + finish(); + } else { + doScan(); + } + // FIXME(Marcus/Mar 10, 2017): May want to synchronize this... + updateTargetAngle(); + } + + private void startAcquisition() { + try { + lidar.acquireRange(); + servo.sendMessage(getNormalizedAngle()); + } catch (IOException e) { + LOGGER.error("Could not read laser:{}", e.getMessage(), e); + } + } + + private float getNormalizedAngle() { + return this.currentAngle / this.servoRange; + } + + private void doScan() { + // Read previous acquisition + try { + float readDistance = lidar.readDistance(); + // Laser was actually shooting at the previous angle, before + // moving - recalculate for that angle + float lastAngle = currentAngle + (lowToHigh ? -request.getStep() - trim : request.getStep() + trim); + scanResult.addPoint(Point2f.fromPolar(readDistance, (float) Math.toRadians(lastAngle))); + servo.sendMessage(getNormalizedAngle()); + lidar.acquireRange(); + } catch (IOException e) { + LOGGER.error("Could not read laser:{}", e.getMessage(), e); + } + } + + private void finish() { + if (!finished) { + recipient.sendMessage(scanResult); + finished = true; + JfrUtils.end(scanEvent); + JfrUtils.commit(scanEvent); + } else { + LOGGER.warn("Tried to scan more laser points after being finished!"); + } + } + + private void updateTargetAngle() { + if (lowToHigh) { + currentAngle += request.getStep(); + } else { + currentAngle -= request.getStep(); + } + } + + private int calculateNumberOfScans() { + return Math.round(request.getRange() / request.getStep()); + } + + // FIXME(Marcus/Mar 10, 2017): Calculate the required delay later from + // physical model. + private long calculateDelay(float minimumAcquisitionTime, float minimumServoMovementTime) { + float delayPerStep = minimumServoMovementTime * 1000 / calculateNumberOfScans(); + // If we have a slow servo, we will need to wait for the servo to + // move. If we have a slow acquisition, we will need to wait for the + // laser before continuing + float actualDelay = Math.max(delayPerStep, minimumAcquisitionTime); + return Math.round(actualDelay * 1000.0d); + } + + private String getScanInfo() { + return String.format("start: %2.1f, end: %2.1f, step:%2.1f", request.getStartAngle(), + request.getStartAngle() + request.getRange(), request.getStep()); + } + } + + private final static class ScanFixedAngleJob implements Runnable { + private final RoboReference recipient; + private final LidarLiteDevice lidar; + private final float angle; + private final Predicate filter; + private volatile boolean firstRun = true; + + public ScanFixedAngleJob(RoboReference recipient, LidarLiteDevice lidar, float angle, Predicate filter) { + this.recipient = recipient; + this.lidar = lidar; + this.angle = angle; + this.filter = filter; + } + + @Override + public void run() { + try { + if (firstRun) { + firstRun = false; + lidar.acquireRange(); + } else { + ScanResultImpl scan = new ScanResultImpl(1, 0f, filter); + scan.addPoint(lidar.readDistance(), (float) Math.toRadians(angle)); + recipient.sendMessage(scan); + } + } catch (IOException e) { + LOGGER.error("Failed to read lidar:{}", e.getMessage(), e); + } + } + } + + public LaserScanner(RoboContext context, String id) { + super(ScanRequest.class, context, id); + } + + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + pan = configuration.getString("servo", "laserscanner.servo"); + + // Using degrees for convenience + servoRange = (float) configuration.getFloat("servoRange", 45.0f); + + // Using angular degrees per second. + angularSpeed = configuration.getFloat("angularSpeed", 90.0f); + + // Minimum acquisition time, in ms + minimumAcquisitionTime = configuration.getFloat("minAquisitionTime", 2.0f); + + // Trim to align left to right and right to left scans (in degrees) + trim = configuration.getFloat("trim", 0.0f); + + // Set the default minimal range to filter out + float minFilterRange = configuration.getFloat("minFilterRange", DEFAULT_FILTER_MIN_RANGE); + pointFilter = new MinRangePointFilter(minFilterRange); + + try { + lidar = new LidarLiteDevice(getBus(), getAddress()); + } catch (IOException e) { + throw new ConfigurationException(String.format( + "Failed to initialize lidar device. Make sure it is hooked up to bus: %d address: %xd", getBus(), getAddress()), e); + } + } + + @Override + public void onMessage(ScanRequest message) { + RoboReference servo = getPanServo(); + if (message.getRange() == 0 || message.getStep() == 0) { + scheduleFixPanScan(message, servo); + } else { + scheduleScan(message, servo); + } + } + + private void scheduleFixPanScan(ScanRequest message, RoboReference servo) { + servo.sendMessage(message.getStartAngle() / servoRange); + ScanFixedAngleJob fixedAngleJob = new ScanFixedAngleJob(message.getReceiver(), lidar, message.getStartAngle(), pointFilter); + // FIXME(Marcus/Sep 5, 2017): We should try to calculate this - we are + // now assuming that it will take little time to move the servo to the + // "new" position, however, the fact is that the new position will + // usually be the old position. + getContext().getScheduler().schedule(fixedAngleJob, 31, TimeUnit.MILLISECONDS); + getContext().getScheduler().schedule(fixedAngleJob, (long) (31 + minimumAcquisitionTime), TimeUnit.MILLISECONDS); + } + + private RoboReference getPanServo() { + RoboReference servo = getReference(pan); + return servo; + } + + private void scheduleScan(ScanRequest message, RoboReference servo) { + float currentInput = getCurrentInput(servo); + float midPoint = message.getStartAngle() + message.getRange() / 2; + boolean lowToHigh = false; + if (inputToAngle(currentInput) <= midPoint) { + lowToHigh = true; + } + float minimumServoMovementTime = message.getRange() / angularSpeed; + ScanJob job = new ScanJob(lowToHigh, minimumServoMovementTime, minimumAcquisitionTime, trim, message, servo, servoRange, lidar, + message.getReceiver(), pointFilter); + schedule(job); + } + + private float inputToAngle(float currentInput) { + return servoRange * currentInput; + } + + private void schedule(ScanJob job) { + long actualDelayMicros = job.delayMicros; + // One extra for first servo move. + for (int i = 0; i < job.numberOfScans + 1; i++) { + // FIXME(Marcus/Apr 4, 2017): Simplified - need to take angular + // speed of the servo into account. + getContext().getScheduler().schedule(job, actualDelayMicros, TimeUnit.MICROSECONDS); + actualDelayMicros += job.delayMicros; + } + } + + private float getCurrentInput(RoboReference servo) { + try { + return servo.getAttribute(PCA9685ServoUnit.ATTRIBUTE_SERVO_INPUT).get(); + } catch (InterruptedException | ExecutionException e) { + LOGGER.error("Could not read servo input:{}", e.getMessage(), e); + return 0; + } + } + + private RoboReference getReference(String unit) { + if (unit.equals("null")) { + return null; + } + return getContext().getReference(unit); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java index 08620608..2a51e070 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pad/LF710PadUnit.java @@ -27,7 +27,8 @@ import com.robo4j.hw.rpi.pad.LF710Pad; import com.robo4j.hw.rpi.pad.PadInputResponseListener; import com.robo4j.hw.rpi.pad.RoboControlPad; -import com.robo4j.logging.SimpleLoggingUtil; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import java.nio.file.Paths; @@ -37,8 +38,8 @@ * @author Marcus Hirt (@hirt) * @author Miro Wengner (@miragemiko) */ -public class LF710PadUnit extends RoboUnit{ - +public class LF710PadUnit extends RoboUnit { + private static final Logger LOGGER = LoggerFactory.getLogger(LF710PadUnit.class); private RoboControlPad pad; private LF710ButtonObserver observer; private PadInputResponseListener listener; @@ -54,7 +55,7 @@ protected void onInitialization(Configuration configuration) throws Configuratio var input = configuration.getString("input", null); target = configuration.getString("target", null); - if(input == null){ + if (input == null) { throw ConfigurationException.createMissingConfigNameException("input"); } if (target == null) { @@ -71,9 +72,9 @@ public void start() { pad.connect(); observer = new LF710ButtonObserver(pad); listener = (LF710Message response) -> { - if(getState() == LifecycleState.STARTED){ - if(targetRef == null){ - SimpleLoggingUtil.info(getClass(), "PAD pressed: " + response); + if (getState() == LifecycleState.STARTED) { + if (targetRef == null) { + LOGGER.warn("PAD pressed:{}", response); } else { targetRef.sendMessage(response); } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/MC33926HBridgeUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/MC33926HBridgeUnit.java index 9ed0a7cf..1b4db626 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/MC33926HBridgeUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/MC33926HBridgeUnit.java @@ -16,106 +16,106 @@ */ package com.robo4j.units.rpi.pwm; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.pwm.HBridgeMC33926Device; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device; import com.robo4j.hw.rpi.utils.GpioPin; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRegistry; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Motor unit associated with the {@link HBridgeMC33926Device} driver. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class MC33926HBridgeUnit extends I2CRoboUnit { - /** - * The key used to configure which channel to use. - */ - public static final String CONFIGURATION_KEY_CHANNEL = "channel"; - - /** - * The key used to configure the symbolic name. - */ - public static final String CONFIGURATION_KEY_NAME = "name"; - - /** - * The key used to configure the gpio RaspiPin to use for "IN1". Use the - * name, such as GPIO_01. - */ - public static final String CONFIGURATION_KEY_GPIO_IN_1 = "in1"; - - /** - * The key used to configure the gpio pin to use for "IN2". Use the name, - * such as GPIO_02. - */ - public static final String CONFIGURATION_KEY_GPIO_IN_2 = "in2"; - - /** - * The key used to invert the direction of the motor. The value if the key - * is not present defaults to false. - */ - public static final String CONFIGURATION_KEY_INVERT = "invert"; - - /** - * The engine. - */ - private HBridgeMC33926Device engine; - - public MC33926HBridgeUnit(RoboContext context, String id) { - super(Float.class, context, id); - } - - /** - * - * @param configuration - * unit configuration - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - - PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), - () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); - - int channel = configuration.getInteger(CONFIGURATION_KEY_CHANNEL, -1); - if (channel == -1) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_CHANNEL); - } - - String in1Name = configuration.getString(CONFIGURATION_KEY_GPIO_IN_1, null); - if (in1Name == null) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_GPIO_IN_1); - } - var gpioIn1 = GpioPin.getByName(in1Name); - - String in2Name = configuration.getString(CONFIGURATION_KEY_GPIO_IN_1, null); - if (in2Name == null) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_GPIO_IN_1); - } - var gpioIn2 = GpioPin.getByName(in2Name);; - - boolean invert = configuration.getBoolean(CONFIGURATION_KEY_INVERT, false); - - // TODO: improve configuraiton - engine = new HBridgeMC33926Device(configuration.getString(CONFIGURATION_KEY_NAME, "MC33926"), pcaDevice.getChannel(channel), gpioIn1, - gpioIn2, invert); - } - - @Override - public void onMessage(Float message) { - try { - engine.setSpeed(message); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to set motor speed to " + message, e); - } - } + /** + * The key used to configure which channel to use. + */ + public static final String CONFIGURATION_KEY_CHANNEL = "channel"; + + /** + * The key used to configure the symbolic name. + */ + public static final String CONFIGURATION_KEY_NAME = "name"; + + /** + * The key used to configure the gpio RaspiPin to use for "IN1". Use the + * name, such as GPIO_01. + */ + public static final String CONFIGURATION_KEY_GPIO_IN_1 = "in1"; + + /** + * The key used to configure the gpio pin to use for "IN2". Use the name, + * such as GPIO_02. + */ + public static final String CONFIGURATION_KEY_GPIO_IN_2 = "in2"; + + /** + * The key used to invert the direction of the motor. The value if the key + * is not present defaults to false. + */ + public static final String CONFIGURATION_KEY_INVERT = "invert"; + + private static final Logger LOGGER = LoggerFactory.getLogger(MC33926HBridgeUnit.class); + /** + * The engine. + */ + private HBridgeMC33926Device engine; + + public MC33926HBridgeUnit(RoboContext context, String id) { + super(Float.class, context, id); + } + + /** + * @param configuration unit configuration + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + + PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), + () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); + + int channel = configuration.getInteger(CONFIGURATION_KEY_CHANNEL, -1); + if (channel == -1) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_CHANNEL); + } + + String in1Name = configuration.getString(CONFIGURATION_KEY_GPIO_IN_1, null); + if (in1Name == null) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_GPIO_IN_1); + } + var gpioIn1 = GpioPin.getByName(in1Name); + + String in2Name = configuration.getString(CONFIGURATION_KEY_GPIO_IN_1, null); + if (in2Name == null) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_GPIO_IN_1); + } + var gpioIn2 = GpioPin.getByName(in2Name); + ; + + boolean invert = configuration.getBoolean(CONFIGURATION_KEY_INVERT, false); + + // TODO: improve configuraiton + engine = new HBridgeMC33926Device(configuration.getString(CONFIGURATION_KEY_NAME, "MC33926"), pcaDevice.getChannel(channel), gpioIn1, + gpioIn2, invert); + } + + @Override + public void onMessage(Float message) { + try { + engine.setSpeed(message); + } catch (IOException e) { + LOGGER.error("Failed to set motor speed to:{}", message, e); + } + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/PCA9685ServoUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/PCA9685ServoUnit.java index d41b7e58..dc9932a5 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/PCA9685ServoUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/pwm/PCA9685ServoUnit.java @@ -16,10 +16,6 @@ */ package com.robo4j.units.rpi.pwm; -import java.io.IOException; -import java.util.Collection; -import java.util.Collections; - import com.robo4j.AttributeDescriptor; import com.robo4j.ConfigurationException; import com.robo4j.DefaultAttributeDescriptor; @@ -27,129 +23,127 @@ import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.pwm.PCA9685Servo; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRegistry; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; +import java.util.Collection; +import java.util.Collections; /** * Servo unit associated with the PCA9685 PWM driver. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class PCA9685ServoUnit extends I2CRoboUnit { - /** - * The key used to configure which channel to use. - */ - public static final String CONFIGURATION_KEY_CHANNEL = "channel"; - /** - * The key used to configure how much trim to use. - */ - public static final String CONFIGURATION_KEY_TRIM = "trim"; - /** - * The key used to configure if the servo should be inverted. - */ - public static final String CONFIGURATION_KEY_INVERTED = "inverted"; - /** - * The key used to configure the dual rate to use. - */ - public static final String CONFIGURATION_KEY_DUAL_RATE = "dualRate"; - /** - * The key used to configure the expo to use. - */ - public static final String CONFIGURATION_KEY_EXPO = "expo"; - /** - * The setting to reset to on shutdown. If this is not set, nothing will - * happen on shutdown. - */ - public static final String CONFIGURATION_KEY_SHUTDOWN_VALUE = "shutdownValue"; - - public static final AttributeDescriptor ATTRIBUTE_SERVO_INPUT = DefaultAttributeDescriptor.create(Float.class, "input"); - public static final Collection> KNOWN_ATTRIBUTES = Collections.singleton(ATTRIBUTE_SERVO_INPUT); + /** + * The key used to configure which channel to use. + */ + public static final String CONFIGURATION_KEY_CHANNEL = "channel"; + /** + * The key used to configure how much trim to use. + */ + public static final String CONFIGURATION_KEY_TRIM = "trim"; + /** + * The key used to configure if the servo should be inverted. + */ + public static final String CONFIGURATION_KEY_INVERTED = "inverted"; + /** + * The key used to configure the dual rate to use. + */ + public static final String CONFIGURATION_KEY_DUAL_RATE = "dualRate"; + /** + * The key used to configure the expo to use. + */ + public static final String CONFIGURATION_KEY_EXPO = "expo"; + /** + * The setting to reset to on shutdown. If this is not set, nothing will + * happen on shutdown. + */ + public static final String CONFIGURATION_KEY_SHUTDOWN_VALUE = "shutdownValue"; - private PCA9685Servo servo; - private Integer channel; - private Float shutdownValue; + public static final AttributeDescriptor ATTRIBUTE_SERVO_INPUT = DefaultAttributeDescriptor.create(Float.class, "input"); + public static final Collection> KNOWN_ATTRIBUTES = Collections.singleton(ATTRIBUTE_SERVO_INPUT); + private static final Logger LOGGER = LoggerFactory.getLogger(PCA9685ServoUnit.class); + private PCA9685Servo servo; + private Integer channel; + private Float shutdownValue; - /** - * Constructor. - * - * @param context - * the RoboContext in which to define the unit - * @param id - * the id of the unit - */ - public PCA9685ServoUnit(RoboContext context, String id) { - super(Float.class, context, id); - } + /** + * Constructor. + * + * @param context the RoboContext in which to define the unit + * @param id the id of the unit + */ + public PCA9685ServoUnit(RoboContext context, String id) { + super(Float.class, context, id); + } - /** - * - * @param configuration - * unit configuration - * @throws ConfigurationException - * exception - */ - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), - () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); - channel = configuration.getInteger(CONFIGURATION_KEY_CHANNEL, -1); - if (channel == -1) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_CHANNEL); - } - servo = new PCA9685Servo(pcaDevice.getChannel(channel)); - servo.setTrim(configuration.getFloat(CONFIGURATION_KEY_TRIM, 0f)); - servo.setInverted(configuration.getBoolean(CONFIGURATION_KEY_INVERTED, false)); - servo.setDualRate(configuration.getFloat(CONFIGURATION_KEY_DUAL_RATE, 1.0f)); - servo.setExpo(configuration.getFloat(CONFIGURATION_KEY_EXPO, 0.0f)); - shutdownValue = configuration.getFloat(CONFIGURATION_KEY_SHUTDOWN_VALUE, null); - } + /** + * @param configuration unit configuration + * @throws ConfigurationException exception + */ + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), + () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); + channel = configuration.getInteger(CONFIGURATION_KEY_CHANNEL, -1); + if (channel == -1) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_CHANNEL); + } + servo = new PCA9685Servo(pcaDevice.getChannel(channel)); + servo.setTrim(configuration.getFloat(CONFIGURATION_KEY_TRIM, 0f)); + servo.setInverted(configuration.getBoolean(CONFIGURATION_KEY_INVERTED, false)); + servo.setDualRate(configuration.getFloat(CONFIGURATION_KEY_DUAL_RATE, 1.0f)); + servo.setExpo(configuration.getFloat(CONFIGURATION_KEY_EXPO, 0.0f)); + shutdownValue = configuration.getFloat(CONFIGURATION_KEY_SHUTDOWN_VALUE, null); + } - /** - * -1 to 1 - * - * @param message - * the message received by this unit. - * - */ - @Override - public void onMessage(Float message) { - try { - servo.setInput(message); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not set servo input", e); - } - } + /** + * -1 to 1 + * + * @param message the message received by this unit. + */ + @Override + public void onMessage(Float message) { + try { + servo.setInput(message); + } catch (IOException e) { + LOGGER.error("Could not set servo input:{}", e.getMessage(), e); + } + } - @SuppressWarnings("unchecked") - @Override - protected R onGetAttribute(AttributeDescriptor descriptor) { - if (descriptor.getAttributeName().equals("input") && descriptor.getAttributeType() == Float.class) { - try { - return (R) Float.valueOf(servo.getInput()); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Failed to read servo input", e); - } - } - return super.onGetAttribute(descriptor); - } + @SuppressWarnings("unchecked") + @Override + protected R onGetAttribute(AttributeDescriptor descriptor) { + if (descriptor.getAttributeName().equals("input") && descriptor.getAttributeType() == Float.class) { + try { + return (R) Float.valueOf(servo.getInput()); + } catch (IOException e) { + LOGGER.error("Failed to read servo input:{}", e.getMessage(), e); + } + } + return super.onGetAttribute(descriptor); + } - @Override - public Collection> getKnownAttributes() { - return KNOWN_ATTRIBUTES; - } + @Override + public Collection> getKnownAttributes() { + return KNOWN_ATTRIBUTES; + } - @Override - public void shutdown() { - if (shutdownValue != null) { - try { - servo.setInput(shutdownValue.floatValue()); - } catch (IOException e) { - SimpleLoggingUtil.debug(PCA9685ServoUnit.class, "Failed to set the shutdown value!", e); - } - } - super.shutdown(); - } + @Override + public void shutdown() { + if (shutdownValue != null) { + try { + servo.setInput(shutdownValue.floatValue()); + } catch (IOException e) { + LOGGER.error("Failed to set the shutdown value:{}", e.getMessage(), e); + } + } + super.shutdown(); + } } diff --git a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/roboclaw/RoboClawRCTankUnit.java b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/roboclaw/RoboClawRCTankUnit.java index a64b5cb3..e2ec62c6 100644 --- a/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/roboclaw/RoboClawRCTankUnit.java +++ b/robo4j-units-rpi/src/main/java/com/robo4j/units/rpi/roboclaw/RoboClawRCTankUnit.java @@ -16,97 +16,97 @@ */ package com.robo4j.units.rpi.roboclaw; -import java.io.IOException; - import com.robo4j.ConfigurationException; import com.robo4j.RoboContext; import com.robo4j.configuration.Configuration; import com.robo4j.hw.rpi.i2c.pwm.PCA9685Servo; import com.robo4j.hw.rpi.i2c.pwm.PWMPCA9685Device; import com.robo4j.hw.rpi.pwm.roboclaw.RoboClawRCTank; -import com.robo4j.logging.SimpleLoggingUtil; import com.robo4j.units.rpi.I2CRegistry; import com.robo4j.units.rpi.I2CRoboUnit; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.io.IOException; /** * Configurable unit for a RoboClaw configured with two engines, controlled * using PWM signals from a PCA9685. - * + * * @author Marcus Hirt (@hirt) * @author Miroslav Wengner (@miragemiko) */ public class RoboClawRCTankUnit extends I2CRoboUnit { - /** - * The key used to configure which channel to use for the left engine. - */ - public static String CONFIGURATION_KEY_LEFT_CHANNEL = "leftChannel"; + /** + * The key used to configure which channel to use for the left engine. + */ + public static String CONFIGURATION_KEY_LEFT_CHANNEL = "leftChannel"; - /** - * The key used to configure if the channel for the left engine needs to be - * inverted. - */ - public static String CONFIGURATION_KEY_LEFT_INVERTED = "leftInverted"; + /** + * The key used to configure if the channel for the left engine needs to be + * inverted. + */ + public static String CONFIGURATION_KEY_LEFT_INVERTED = "leftInverted"; - /** - * The key used to configure which channel to use for the right engine. - */ - public static String CONFIGURATION_KEY_RIGHT_CHANNEL = "rightChannel"; + /** + * The key used to configure which channel to use for the right engine. + */ + public static String CONFIGURATION_KEY_RIGHT_CHANNEL = "rightChannel"; - /** - * The key used to configure if the channel for the left engine needs to be - * inverted. - */ - public static String CONFIGURATION_KEY_RIGHT_INVERTED = "rightInverted"; + /** + * The key used to configure if the channel for the left engine needs to be + * inverted. + */ + public static String CONFIGURATION_KEY_RIGHT_INVERTED = "rightInverted"; - private RoboClawRCTank tank; + private static final Logger LOGGER = LoggerFactory.getLogger(RoboClawRCTankUnit.class); + private RoboClawRCTank tank; - /** - * Constructor. - * - * @param context - * the RoboContext in which to define the unit - * @param id - * the id of the unit - */ - public RoboClawRCTankUnit(RoboContext context, String id) { - super(MotionEvent.class, context, id); - } + /** + * Constructor. + * + * @param context the RoboContext in which to define the unit + * @param id the id of the unit + */ + public RoboClawRCTankUnit(RoboContext context, String id) { + super(MotionEvent.class, context, id); + } - @Override - protected void onInitialization(Configuration configuration) throws ConfigurationException { - super.onInitialization(configuration); - PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), - () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); - int leftChannel = configuration.getInteger(CONFIGURATION_KEY_LEFT_CHANNEL, -1); - if (leftChannel == -1) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_LEFT_CHANNEL); - } - int rightChannel = configuration.getInteger(CONFIGURATION_KEY_RIGHT_CHANNEL, -1); - if (rightChannel == -1) { - throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_RIGHT_CHANNEL); - } - boolean leftInvert = configuration.getBoolean(CONFIGURATION_KEY_LEFT_INVERTED, false); - boolean rightInvert = configuration.getBoolean(CONFIGURATION_KEY_RIGHT_INVERTED, false); + @Override + protected void onInitialization(Configuration configuration) throws ConfigurationException { + super.onInitialization(configuration); + PWMPCA9685Device pcaDevice = I2CRegistry.createAndRegisterIfAbsent(getBus(), getAddress(), + () -> PWMPCA9685Device.createDevice(getBus(), getAddress())); + int leftChannel = configuration.getInteger(CONFIGURATION_KEY_LEFT_CHANNEL, -1); + if (leftChannel == -1) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_LEFT_CHANNEL); + } + int rightChannel = configuration.getInteger(CONFIGURATION_KEY_RIGHT_CHANNEL, -1); + if (rightChannel == -1) { + throw ConfigurationException.createMissingConfigNameException(CONFIGURATION_KEY_RIGHT_CHANNEL); + } + boolean leftInvert = configuration.getBoolean(CONFIGURATION_KEY_LEFT_INVERTED, false); + boolean rightInvert = configuration.getBoolean(CONFIGURATION_KEY_RIGHT_INVERTED, false); - PCA9685Servo leftServo = new PCA9685Servo(pcaDevice.getChannel(leftChannel)); - leftServo.setInverted(leftInvert); - PCA9685Servo rightServo = new PCA9685Servo(pcaDevice.getChannel(rightChannel)); - rightServo.setInverted(rightInvert); - try { - tank = new RoboClawRCTank(leftServo, rightServo); - } catch (IOException e) { - throw new ConfigurationException("Could not initiate device!", e); - } - } + PCA9685Servo leftServo = new PCA9685Servo(pcaDevice.getChannel(leftChannel)); + leftServo.setInverted(leftInvert); + PCA9685Servo rightServo = new PCA9685Servo(pcaDevice.getChannel(rightChannel)); + rightServo.setInverted(rightInvert); + try { + tank = new RoboClawRCTank(leftServo, rightServo); + } catch (IOException e) { + throw new ConfigurationException("Could not initiate device!", e); + } + } - @Override - public void onMessage(MotionEvent message) { - super.onMessage(message); - try { - tank.setDirection(message.getDirection()); - tank.setSpeed(message.getSpeed()); - } catch (IOException e) { - SimpleLoggingUtil.error(getClass(), "Could not set speed and/or direction to " + message, e); - } - } + @Override + public void onMessage(MotionEvent message) { + super.onMessage(message); + try { + tank.setDirection(message.getDirection()); + tank.setSpeed(message.getSpeed()); + } catch (IOException e) { + LOGGER.error("Could not set speed and/or direction to:{}", message, e); + } + } }