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); + } + } }