diff --git a/.settings/org.eclipse.jdt.core.prefs b/.settings/org.eclipse.jdt.core.prefs new file mode 100644 index 0000000..b080d2d --- /dev/null +++ b/.settings/org.eclipse.jdt.core.prefs @@ -0,0 +1,4 @@ +eclipse.preferences.version=1 +org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6 +org.eclipse.jdt.core.compiler.compliance=1.6 +org.eclipse.jdt.core.compiler.source=1.6 diff --git a/src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java b/src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java new file mode 100644 index 0000000..5cdcfc9 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/networkdata/MotorEvent.java @@ -0,0 +1,37 @@ +package ve.ucv.ciens.ccg.networkdata; + +import java.io.Serializable; + +public class MotorEvent implements Serializable{ + private static final long serialVersionUID = 9989L; + + public enum motor_t {NONE, MOTOR_A, MOTOR_B, MOTOR_C, MOTOR_AC}; + + private motor_t motor; + private byte power; + + public MotorEvent(){ + motor = motor_t.NONE; + power = 0; + } + + public void setMotor(motor_t motor){ + this.motor = motor; + } + + public void setPower(byte power) throws IllegalArgumentException{ + if(power > 100 || power < -100){ + throw new IllegalArgumentException("Motor power must be a number between -100 and 100"); + }else{ + this.power = power; + } + } + + public motor_t getMotor(){ + return this.motor; + } + + public byte getPower(){ + return this.power; + } +} diff --git a/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java b/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java new file mode 100644 index 0000000..a2911f9 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/networkdata/MotorEventACK.java @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2014 Miguel Angel Astor Romero + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package ve.ucv.ciens.ccg.networkdata; + +import java.io.Serializable; + +public class MotorEventACK implements Serializable { + private static final long serialVersionUID = 9989L; + + private boolean clientQueueIsFull; + + public MotorEventACK(boolean isQueueFull){ + this.clientQueueIsFull = isQueueFull; + } + + public boolean isClientQueueFull(){ + return this.clientQueueIsFull; + } +} diff --git a/src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java b/src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java new file mode 100644 index 0000000..8f8617c --- /dev/null +++ b/src/ve/ucv/ciens/ccg/networkdata/SensorDataMessage.java @@ -0,0 +1,8 @@ +package ve.ucv.ciens.ccg.networkdata; + +import java.io.Serializable; + +public class SensorDataMessage implements Serializable{ + private static final long serialVersionUID = 9989L; + +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java index e02b331..48551d3 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/CamActivity.java @@ -16,8 +16,8 @@ package ve.ucv.ciens.ccg.nxtcam; import ve.ucv.ciens.ccg.nxtcam.camera.CameraPreview; -import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread; import ve.ucv.ciens.ccg.nxtcam.network.LCPThread; +import ve.ucv.ciens.ccg.nxtcam.network.VideoStreamingThread; import ve.ucv.ciens.ccg.nxtcam.utils.Logger; import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; import android.app.Activity; @@ -56,6 +56,9 @@ public class CamActivity extends Activity{ serverIp = intent.getStringExtra("address"); imThread = new VideoStreamingThread(serverIp); imThread.start(); + + botThread = new LCPThread(serverIp); + botThread.start(); } @Override @@ -104,14 +107,17 @@ public class CamActivity extends Activity{ releaseCamera(); } } - + @Override public void onDestroy(){ super.onDestroy(); - // TODO: Destroy the network threads. + imThread.finish(); imThread = null; + + botThread.finish(); + botThread = null; } - + @Override public void onBackPressed(){ Intent result = new Intent(); diff --git a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java b/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java index 880912d..651062b 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/MainActivity.java @@ -19,7 +19,6 @@ import java.io.IOException; import java.net.DatagramPacket; import java.net.InetAddress; import java.net.MulticastSocket; -import java.net.Socket; import ve.ucv.ciens.ccg.nxtcam.dialogs.ConnectRobotDialog; import ve.ucv.ciens.ccg.nxtcam.dialogs.ConnectRobotDialog.ConnectRobotDialogListener; @@ -324,7 +323,7 @@ public class MainActivity extends Activity implements WifiOnDialogListener, Conn public ServiceDiscoveryTask(){ // Open a multicast socket and join the project's multicast group. try{ - udpSocket = new MulticastSocket(ProjectConstants.SERVER_UDP_PORT); + udpSocket = new MulticastSocket(ProjectConstants.SERVICE_DISCOVERY_PORT); InetAddress group = InetAddress.getByName(ProjectConstants.MULTICAST_ADDRESS); udpSocket.joinGroup(group); }catch(IOException io){ diff --git a/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java b/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java index 176aa17..d90f08a 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/camera/CameraPreview.java @@ -108,19 +108,19 @@ public class CameraPreview extends SurfaceView implements SurfaceHolder.Callback camParams.getSupportedPreviewSizes(); List sizes = camParams.getSupportedPreviewSizes(); - /*for(Size size: sizes){ + for(Size size: sizes){ Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Supported preview size (" + size.width + ", " + size.height + ")"); } - Size optimal = getOptimalPreviewSize(sizes, w, h); + /*Size optimal = getOptimalPreviewSize(sizes, w, h); Logger.log_d(TAG, CLASS_NAME + ".surfaceChanged() :: Preview size set at (" + optimal.width + ", " + optimal.height + ")"); camParams.setPreviewSize(optimal.width, optimal.height);*/ - camParams.setPreviewSize(720, 480); + camParams.setPreviewSize(352, 288); camera.setParameters(camParams); /*previewWidth = optimal.width; previewHeight = optimal.height;*/ - previewWidth = 720; - previewHeight = 480; + previewWidth = 352; + previewHeight = 288; android.hardware.Camera.CameraInfo info = new android.hardware.Camera.CameraInfo(); android.hardware.Camera.getCameraInfo(0, info); diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java b/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java index 6f6eedb..bf7ba25 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/BTCommunicator.java @@ -228,9 +228,6 @@ public class BTCommunicator{ if(connected){ try{ byte[] message = new byte[bytes]; - for(int i = 0; i < message.length; ++i){ - message[i] = 0x00; - } nxtInputStream.read(message, 0, bytes); return message; }catch(IOException e){ diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java b/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java deleted file mode 100644 index b330f8e..0000000 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/BluetoothManager.java +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Copyright (C) 2013 Miguel Angel Astor Romero - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package ve.ucv.ciens.ccg.nxtcam.network; - -import java.io.IOException; -import java.io.InputStream; -import java.io.OutputStream; -import java.util.Set; -import java.util.UUID; - -import android.bluetooth.BluetoothAdapter; -import android.bluetooth.BluetoothDevice; -import android.bluetooth.BluetoothSocket; -import android.util.Log; - -public class BluetoothManager{ - private static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB"); - private static final String OUI_LEGO = "00:16:53"; - private static final String TAG = "BTMNGR"; - - private boolean connected; - private BluetoothAdapter bt_adapter; - private BluetoothSocket bt_socket = null; - private OutputStream nxt_out_stream = null; - private InputStream nxt_in_stream = null; - - private static class SingletonHolder{ - public static final BluetoothManager INSTANCE = new BluetoothManager(); - } - - private BluetoothManager(){ - connected = false; - bt_adapter = BluetoothAdapter.getDefaultAdapter(); - bt_socket = null; - nxt_in_stream = null; - nxt_out_stream = null; - } - - public static BluetoothManager getInstance(){ - return SingletonHolder.INSTANCE; - } - - public boolean isBTSupported(){ - return bt_adapter != null; - } - - public boolean isConnected(){ - return connected; - } - - public boolean isBTEnabled(){ - return bt_adapter.isEnabled(); - } - - public void disableBT(){ - bt_adapter.disable(); - } - - public Set getPairedDevices(){ - return bt_adapter.getBondedDevices(); - } - - /** - * Sets up a connection with a NXT device. - * - * Verifies if the target device is a valid NXT robot by checking agains Lego's OUI. - * Also creates the socket and the streams associated with the connection - * - * @param mac_address The mac address of the target device. - * @return true if the connection was established succesfully, otherwise false. - * @throws IOException - */ - public boolean establishConnection(String mac_address) throws IOException{ - if (!bt_adapter.isEnabled()){ - return false; - } - if(connected){ - return false; - } - if(bt_adapter.isEnabled()){ - if(mac_address == "NONE"){ - return false; - }else{ - if(mac_address.substring(0, 8).compareTo(OUI_LEGO) != 0){ - Log.d(TAG, "establishConnection() :: Not a Lego MAC. Prefix : " + mac_address.substring(0, 8) + " :: OUI : " + OUI_LEGO); - return false; - }else{ - try{ - Log.d(TAG, "establishConnection() :: Getting device with mac address: " + mac_address); - BluetoothDevice nxtDevice = null; - nxtDevice = bt_adapter.getRemoteDevice(mac_address); - if (nxtDevice == null) { - Log.e(TAG, "establishConnection() :: No device found."); - throw new IOException(); - } - - Log.d(TAG, "establishConnection() :: Opening socket."); - bt_socket = nxtDevice.createRfcommSocketToServiceRecord(SERIAL_PORT_SERVICE_CLASS_UUID); - Log.d(TAG, "establishConnection() :: Connecting."); - bt_socket.connect(); - - Log.d(TAG, "establishConnection() :: Opening IO streams."); - nxt_in_stream = bt_socket.getInputStream(); - nxt_out_stream = bt_socket.getOutputStream(); - - Log.d(TAG, "establishConnection() :: Connection established."); - connected = true; - - }catch(IOException e){ - Log.e(TAG, "establishConnection() :: Connection failed."); - Log.e(TAG, Log.getStackTraceString(e)); - connected = false; - throw e; - } - return connected; - } - } - } - return false; - } - - /** - * Closes the active connection if any. - * - * Additionally clears the socket and the streams associated to said connection. - * - * @return true if the connection was succesfully closed; false if no connection exists. - * @throws IOException - */ - public boolean stopConnection() throws IOException{ - try{ - if(bt_socket != null){ - Log.d(TAG, "stopConnection() :: Closing connection."); - bt_socket.close(); - bt_socket = null; - nxt_in_stream = null; - nxt_out_stream = null; - connected = false; - Log.d(TAG, "stopConnection() :: Connection closed."); - return true; - } - }catch( IOException e){ - Log.e(TAG, "stopConnection()"); - Log.e(TAG, Log.getStackTraceString(e)); - throw e; - } - return false; - } - - /** - * Sends a message to the NXT robot. - * - * @param message The data to be sent. - * @throws IOException - */ - public synchronized void writeMessage(byte[] message) throws IOException{ - if(connected){ - try{ - nxt_out_stream.write(message); - nxt_out_stream.flush(); - }catch(IOException e){ - Log.e(TAG, "writeMessage()"); - Log.e(TAG, Log.getStackTraceString(e)); - throw e; - } - } - } - - /** - * Reads a message sent by the NXT robot. - * - * @return The data received as a byte[] if a valid connection exists, otherwise null. - * @throws IOException - */ - public synchronized byte[] readMessage(int bytes) throws IOException{ - if(connected){ - try{ - byte[] message = new byte[bytes]; - for(int i = 0; i < message.length; ++i){ - message[i] = 0x00; - } - nxt_in_stream.read(message, 0, bytes); - return message; - }catch(IOException e){ - Log.e(TAG, "readMessage()"); - Log.e(TAG, Log.getStackTraceString(e)); - throw e; - } - }else{ - return null; - } - } -} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java index 335f553..e96ebc9 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/LCPThread.java @@ -15,13 +15,90 @@ */ package ve.ucv.ciens.ccg.nxtcam.network; -public class LCPThread extends Thread{ +import java.io.IOException; - public LCPThread(){ - +import ve.ucv.ciens.ccg.networkdata.MotorEvent; +import ve.ucv.ciens.ccg.networkdata.MotorEvent.motor_t; +import ve.ucv.ciens.ccg.nxtcam.network.protocols.LegoCommunicationProtocol; +import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue; +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; + +public class LCPThread extends Thread{ + private static final String TAG = "LCP_THREAD"; + private static final String CLASS_NAME = LCPThread.class.getSimpleName(); + + private boolean done; + private boolean reportSensors; + private BTCommunicator btComm; + private MotorControlThread motorControl; + private SensorReportThread sensorReport; + + private MotorEventQueue queue; + + public LCPThread(String serverIp){ + super("Robot Control Main Thread"); + btComm = BTCommunicator.getInstance(); + done = false; + motorControl = new MotorControlThread(serverIp); + sensorReport = new SensorReportThread(serverIp); + queue = MotorEventQueue.getInstance(); } - + public void run(){ - + long then, now, delta; + MotorEvent event; + + sensorReport.start(); + motorControl.start(); + + then = System.currentTimeMillis(); + + while(!motorControl.isConnected()){ + now = System.currentTimeMillis(); + delta = now - then; + if(delta > 9000L){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread motorControl could not connect to the server."); + return; + } + } + + if((reportSensors = sensorReport.isConnected())){ + Logger.log_d(TAG, CLASS_NAME + ".run() :: Sensor data can be reported."); + }else{ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Thread sensorReport could not connect to the server."); + Logger.log_e(TAG, CLASS_NAME + ".run() :: Sensor data will not be reported to server app."); + } + + while(!done){ + if(btComm.isBTEnabled() && btComm.isConnected()){ + + event = queue.getNextEvent(); + + try{ + btComm.writeMessage( + LegoCommunicationProtocol.setOutputState( + event.getMotor() == motor_t.MOTOR_A ? LegoCommunicationProtocol.PORT_0 : (event.getMotor() == motor_t.MOTOR_B ? LegoCommunicationProtocol.PORT_1 : LegoCommunicationProtocol.PORT_2), + event.getPower()) + ); + Logger.log_i(TAG, CLASS_NAME + ".run() :: Message sent to the robot."); + + try{ sleep(40); }catch(InterruptedException ie){ } + + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException sending message to the robot: " + io.getMessage()); + } + + if(reportSensors){ + + } + }else{ + Logger.log_e(TAG, CLASS_NAME + ".run() :: The robot disconnected or was never available."); + break; + } + } + } + + public void finish(){ + done = true; } } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java new file mode 100644 index 0000000..cb770b2 --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/MotorControlThread.java @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2014 Miguel Angel Astor Romero + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package ve.ucv.ciens.ccg.nxtcam.network; + +import java.io.IOException; +import java.io.ObjectInputStream; +import java.io.ObjectOutputStream; +import java.net.Socket; + +import ve.ucv.ciens.ccg.networkdata.MotorEvent; +import ve.ucv.ciens.ccg.networkdata.MotorEvent.motor_t; +import ve.ucv.ciens.ccg.networkdata.MotorEventACK; +import ve.ucv.ciens.ccg.nxtcam.robotcontrol.MotorEventQueue; +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; +import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; + +public class MotorControlThread extends Thread { + private static final String TAG = "MOTOR_CONTROL"; + private static final String CLASS_NAME = MotorControlThread.class.getSimpleName(); + + private Socket socket; + private String serverIp; + private MotorEventQueue queue; + private boolean done; + private ObjectInputStream reader; + private ObjectOutputStream writer; + private boolean connected; + + public MotorControlThread(String serverIp){ + super("Motor Control Thread"); + this.serverIp = serverIp; + done = false; + connected = false; + queue = MotorEventQueue.getInstance(); + } + + @Override + public void run(){ + Object msg; + MotorEvent event; + MotorEventACK ack; + + if(!connectToServer()){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: The thread is not connected to a server. Finishing."); + return; + }else{ + while(!done){ + // Receive a message and enqueue it; + msg = readMessage(); + event = verifyMessage(msg); + if(event != null){ + queue.addEvent(event); + Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor control message enqueued."); + Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor ID: " + (event.getMotor() == motor_t.MOTOR_A ? "MOTOR_A" : "MOTOR_C")); + Logger.log_i(TAG, CLASS_NAME + ".run() :: Motor power: " + Byte.toString(event.getPower())); + }else{ + Logger.log_i(TAG, CLASS_NAME + ".run() :: Message could not be verified;"); + } + + // Send corresponding ack; + ack = new MotorEventACK(queue.getSize() >= 10); + try{ + writer.writeObject(ack); + Logger.log_i(TAG, CLASS_NAME + ".run() :: First ACK sent."); + }catch(Exception ex){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Exception while sending first ACK: " + ex.getMessage()); + break; + } + + if(ack.isClientQueueFull()){ + while(queue.getSize() >= 10){ } + + ack = new MotorEventACK(false); + + try{ + writer.writeObject(ack); + }catch(Exception ex){ + Logger.log_i(TAG, CLASS_NAME + ".run() :: Second ACK sent."); + Logger.log_e(TAG, CLASS_NAME + ".run() :: Exception while sending second ACK: " + ex.getMessage()); + break; + } + } + + event = null; + ack = null; + msg = null; + } + try{ + socket.close(); + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException while closing socket: " + io.getMessage()); + } + } + } + + public void finish(){ + done = true; + } + + public boolean connectToServer(){ + try{ + socket = new Socket(serverIp, ProjectConstants.MOTOR_CONTROL_PORT); + reader = new ObjectInputStream(socket.getInputStream()); + writer = new ObjectOutputStream(socket.getOutputStream()); + connected = true; + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: IOException caught: " + io.getMessage()); + connected = false; + } + return connected; + } + + private Object readMessage(){ + Object message; + try{ + message = reader.readObject(); + Logger.log_i(TAG, CLASS_NAME + ".readMessage() :: Motor control message received."); + }catch(ClassNotFoundException cn){ + Logger.log_e(TAG, CLASS_NAME + ".readMessage() :: ClassNotFoundException caught: " + cn.getMessage()); + message = null; + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".readMessage() :: IOException caught: " + io.getMessage()); + message = null; + } + return message; + } + + private MotorEvent verifyMessage(Object message){ + if(message != null && message instanceof MotorEvent){ + Logger.log_i(TAG, CLASS_NAME + ".verifyMessage() :: Valid motor control message received."); + return (MotorEvent)message; + }else{ + return null; + } + } + + public boolean isConnected(){ + return connected; + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java new file mode 100644 index 0000000..5bff72e --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/SensorReportThread.java @@ -0,0 +1,58 @@ +package ve.ucv.ciens.ccg.nxtcam.network; + +import java.io.IOException; +import java.io.ObjectOutputStream; +import java.net.Socket; + +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; +import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; + +public class SensorReportThread extends Thread{ + private static final String TAG = "SENSOR_REPORT"; + private static final String CLASS_NAME = SensorReportThread.class.getSimpleName(); + + private Socket socket; + private String serverIp; + private boolean done; + private ObjectOutputStream writer; + private boolean connected; + + public SensorReportThread(String serverIp){ + super("Sensor Report Thread"); + this.serverIp = serverIp; + done = false; + connected = false; + } + + @Override + public void run(){ + if(connectToServer()){ + while(!done){ + + } + }else{ + Logger.log_e(TAG, CLASS_NAME + ".run() :: Could not connect to the server."); + } + } + + public void finish(){ + done = true; + } + + private boolean connectToServer(){ + boolean connected; + try{ + socket = new Socket(serverIp, ProjectConstants.SENSOR_REPORT_PORT); + writer = new ObjectOutputStream(socket.getOutputStream()); + connected = true; + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: IOException caught: " + io.getMessage()); + connected = false; + } + return connected; + } + + public boolean isConnected(){ + return connected; + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java index 4e34b9c..91bdf27 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/VideoStreamingThread.java @@ -17,15 +17,15 @@ package ve.ucv.ciens.ccg.nxtcam.network; import java.io.ByteArrayOutputStream; import java.io.IOException; -import java.io.ObjectInputStream; import java.io.ObjectOutputStream; +import java.net.DatagramPacket; +import java.net.DatagramSocket; import java.net.InetAddress; import java.net.Socket; +import java.net.UnknownHostException; import ve.ucv.ciens.ccg.networkdata.VideoFrameDataMessage; -import ve.ucv.ciens.ccg.networkdata.VideoStreamingControlMessage; import ve.ucv.ciens.ccg.nxtcam.camera.CameraImageMonitor; -import ve.ucv.ciens.ccg.nxtcam.network.protocols.VideoStreamingProtocol; import ve.ucv.ciens.ccg.nxtcam.utils.Logger; import ve.ucv.ciens.ccg.nxtcam.utils.ProjectConstants; import android.graphics.ImageFormat; @@ -36,222 +36,117 @@ public class VideoStreamingThread extends Thread{ private final String TAG = "IM_THREAD"; private final String CLASS_NAME = VideoStreamingThread.class.getSimpleName(); - private enum ProtocolState_t {WAIT_FOR_ACK, WAIT_FOR_READY, CAN_SEND, END_STREAM}; - private boolean pause, done; private Object threadPauseMonitor; private CameraImageMonitor camMonitor; private Socket socket; - private ObjectOutputStream writer; - private ObjectInputStream reader; + DatagramSocket udpSocket; private String serverIp; - private ProtocolState_t protocolState; private final ByteArrayOutputStream outputStream = new ByteArrayOutputStream(); public VideoStreamingThread(String serverIp){ super("Video Streaming Thread"); this.serverIp = serverIp; - pause = false; done = false; + pause = false; threadPauseMonitor = new Object(); socket = null; - writer = null; - reader = null; camMonitor = CameraImageMonitor.getInstance(); - protocolState = ProtocolState_t.WAIT_FOR_READY; } - /*public void run(){ - byte[] image; - Object tmpMessage; - VideoStreamingControlMessage controlMessage; - VideoFrameDataMessage dataMessage; - final ByteArrayOutputStream outputStream = new ByteArrayOutputStream(); - - connectToServer(); - - if(!socket.isConnected()){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread."); - return; - }else{ - while(!done){ - // checkPause(); - switch(protocolState){ - case WAIT_FOR_READY: - Logger.log_d(TAG, CLASS_NAME + ".run() :: Reading message from server. State is WAIT_FOR_READY."); - tmpMessage = readMessage(); - - if(!validateImageTransferProtocolMessage(tmpMessage)){ - // If the message received is not valid then send an UNRECOGNIZED message to the server. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received an unrecognized protocol message. State WAIT_FOR_READY."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending UNRECOGNIZED message to server."); - sendUnrecognizedMessage(); - - }else{ - // Else if the message passed the validity check then proceed to the next protocol state. - controlMessage = (VideoStreamingControlMessage)tmpMessage; - if(controlMessage.message == VideoStreamingProtocol.FLOW_CONTROL_CONTINUE){ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received FLOW_CONTROL_CONTINUE from the server."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_READY to CAN_SEND."); - protocolState = ProtocolState_t.CAN_SEND; - }else if(controlMessage.message == VideoStreamingProtocol.STREAM_CONTROL_END){ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received STREAM_CONTROL_END from the server."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_READY to END_STREAM."); - protocolState = ProtocolState_t.END_STREAM; - } - } - break; - - case WAIT_FOR_ACK: - Logger.log_d(TAG, CLASS_NAME + ".run() :: Reading message from server. State is WAIT_FOR_ACK."); - tmpMessage = readMessage(); - - if(!validateImageTransferProtocolMessage(tmpMessage)){ - // If the message received is not valid then send an UNRECOGNIZED message to the server. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received an unrecognized protocol message. State WAIT_FOR_ACK."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending UNRECOGNIZED message to server."); - sendUnrecognizedMessage(); - - }else{ - // Else if the message passed the validity check then proceed to the next protocol state. - controlMessage = (VideoStreamingControlMessage)tmpMessage; - if(controlMessage.message == VideoStreamingProtocol.ACK_SEND_NEXT){ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received ACK_SEND_NEXT from the server."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_ACK to CAN_SEND."); - protocolState = ProtocolState_t.CAN_SEND; - }else if(controlMessage.message == VideoStreamingProtocol.ACK_WAIT){ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Received ACK_WAIT from the server."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from WAIT_FOR_ACK to WAIT_FOR_READY."); - protocolState = ProtocolState_t.WAIT_FOR_READY; - }else if(controlMessage.message == VideoStreamingProtocol.STREAM_CONTROL_END){ - protocolState = ProtocolState_t.END_STREAM; - } - } - break; - - case CAN_SEND: - // Get the image and it's parameters from the monitor. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Getting image data."); - Rect imageSize = camMonitor.getImageParameters(); - image = camMonitor.getImageData(); - - // Compress the image as Jpeg. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Compressing image."); - YuvImage yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null); - yuvImage.compressToJpeg(imageSize, 90, outputStream); - - // Prepare the message for sending. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Building message."); - dataMessage = new VideoFrameDataMessage(); - dataMessage.imageWidth = imageSize.width(); - dataMessage.imageHeight = imageSize.height(); - dataMessage.data = outputStream.toByteArray(); - - // Send the message. - try{ - Logger.log_d(TAG, CLASS_NAME + ".run() :: Sending message."); - writer.writeObject(dataMessage); - }catch(IOException io){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: Error sending image to the server: " + io.getMessage()); - } - - // Clean up stuff. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Cleaning."); - yuvImage = null; - image = null; - outputStream.reset(); - dataMessage = null; - imageSize = null; - - Logger.log_d(TAG, CLASS_NAME + ".run() :: Image data successfuly sent."); - Logger.log_d(TAG, CLASS_NAME + ".run() :: Transitioning from CAN_SEND to WAIT_FOR_ACK."); - protocolState = ProtocolState_t.WAIT_FOR_ACK; - break; - - case END_STREAM: - // Simply disconnect from the server. - Logger.log_d(TAG, CLASS_NAME + ".run() :: Ending video stream."); - disconnect(); - done = true; - break; - } - } - } - Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached."); - }*/ - public void run(){ - connectToServer(); - if(!socket.isConnected()){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: Not connected to a server. Finishing thread."); - return; + try{ + udpSocket = new DatagramSocket(); - }else{ - while(!done){ - sendImage(); - try{ - sleep(50L); - }catch(InterruptedException ie){} - } + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException received creating socket " + io.getMessage()); + System.exit(1); } + + while(!done){ + + synchronized (threadPauseMonitor) { + while(pause){ + try{ threadPauseMonitor.wait(); }catch(InterruptedException ie){ }; + } + } + + sendUdp(); + try{ + sleep(50L); + }catch(InterruptedException ie){} + } + + Logger.log_d(TAG, CLASS_NAME + ".run() :: Thread finish reached."); } - private void sendImage(){ + private byte[] int2ByteArray(int integer){ + int shift; + byte[] array = new byte[4]; + for(int i = 0; i < 4; i++){ + shift = i << 3; + array[3 - i] = (byte)((integer & (0xff << shift)) >>> shift); + } + return array; + } + + private void sendUdp(){ + int bufferSize; byte[] image; - YuvImage yuvImage; + byte[] buffer; + byte[] size; + DatagramPacket packet; VideoFrameDataMessage message; Rect imageSize; + YuvImage yuvImage; image = camMonitor.getImageData(); - if(image == null){ - Logger.log_e(TAG, CLASS_NAME + ".sendImage() :: image is null, skipping frame."); - return; - } imageSize = camMonitor.getImageParameters(); - // Compress the image as Jpeg. - Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Compressing image."); yuvImage = new YuvImage(image, ImageFormat.NV21, imageSize.width(), imageSize.height(), null); yuvImage.compressToJpeg(imageSize, 90, outputStream); - Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Building message."); message = new VideoFrameDataMessage(); message.data = outputStream.toByteArray(); message.imageWidth = imageSize.width(); message.imageHeight = imageSize.height(); - try{ - Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Sending message."); - writer.writeObject(message); - writer.flush(); - writer.reset(); - Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Message sent successfully: "); - }catch(IOException io){ - Logger.log_e(TAG, CLASS_NAME + ".sendImage() :: Error sending image to the server: " + io.getMessage()); + outputStream.reset(); - }finally{ - Logger.log_d(TAG, CLASS_NAME + ".sendImage() :: Cleaning."); - outputStream.reset(); - image = null; - yuvImage = null; - message = null; - imageSize = null; - System.gc(); + ByteArrayOutputStream baos = new ByteArrayOutputStream(); + + try{ + ObjectOutputStream oos = new ObjectOutputStream(baos); + oos.writeObject(message); + oos.flush(); + oos.reset(); + }catch(IOException io){ + Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException received while serializing." + io.getMessage()); + return; } - } - private void connectToServer(){ + buffer = baos.toByteArray(); + baos.reset(); + bufferSize = buffer.length; + size = int2ByteArray(bufferSize); + try{ - Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connecting to the server at " + serverIp); - socket = new Socket(InetAddress.getByName(serverIp), ProjectConstants.SERVER_TCP_PORT_1); - writer = new ObjectOutputStream(socket.getOutputStream()); - reader = new ObjectInputStream(socket.getInputStream()); - Logger.log_i(TAG, CLASS_NAME + ".connectToServer() :: Connection successful."); + packet = new DatagramPacket(size, 4, InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT); + udpSocket.send(packet); + + packet = new DatagramPacket(buffer, buffer.length, InetAddress.getByName(serverIp), ProjectConstants.VIDEO_STREAMING_PORT); + udpSocket.send(packet); + + }catch(UnknownHostException uo){ + Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: UnknownHostException received " + uo.getMessage()); + return; }catch(IOException io){ - Logger.log_e(TAG, CLASS_NAME + ".connectToServer() :: Connection failed with message: " + io.getMessage()); + Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException buffer size is " + Integer.toString(buffer.length)); + Logger.log_e(TAG, CLASS_NAME + ".sendUdp() :: IOException received while sending " + io.getMessage()); + return; } } @@ -271,56 +166,10 @@ public class VideoStreamingThread extends Thread{ Logger.log_i(TAG, CLASS_NAME + ".finish() :: Finishing thread."); } - private void checkPause(){ - synchronized (threadPauseMonitor){ - while(pause){ - Logger.log_d(TAG, CLASS_NAME + ".checkPause() :: Pause requested."); - try{ threadPauseMonitor.wait(); }catch(InterruptedException ie){} - } + public void pauseThread(){ + synchronized (threadPauseMonitor) { + pause = true; } - } - - private Object readMessage(){ - Object tmpMessage; - - // Read a message from the server stream. - try{ - tmpMessage = reader.readObject(); - - }catch(IOException io){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException when reading in WAIT_FOR_READY state."); - tmpMessage = null; - return null; - }catch(ClassNotFoundException cn){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: ClassNotFoundException when reading in WAIT_FOR_READY state."); - tmpMessage = null; - return null; - } - - return tmpMessage; - } - - private boolean validateImageTransferProtocolMessage(Object message){ - if(message != null && message instanceof VideoStreamingControlMessage) - return true; - else - return false; - } - - private void sendUnrecognizedMessage(){ - VideoStreamingControlMessage message = new VideoStreamingControlMessage(); - message.message = VideoStreamingProtocol.UNRECOGNIZED; - - try{ - writer.writeObject(message); - }catch(IOException io){ - Logger.log_e(TAG, CLASS_NAME + ".run() :: IOException when writing UNRECOGNIZED in WAIT_FOR_READY state."); - } - Logger.log_d(TAG, CLASS_NAME + ".run() :: UNRECOGNIZED message sent."); - } - - public synchronized void pauseThread(){ - pause = true; Logger.log_d(TAG, CLASS_NAME + ".pauseThread() :: Pausing thread."); } diff --git a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java index 404c63a..f940c17 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/network/protocols/LegoCommunicationProtocol.java @@ -16,7 +16,13 @@ package ve.ucv.ciens.ccg.nxtcam.network.protocols; import java.security.InvalidParameterException; +import java.util.Arrays; +import ve.ucv.ciens.ccg.nxtcam.utils.Logger; + +/** + *

Utility class that provides methods to get Lego Communication Protocol PDUs as byte arrays.

+ **/ public abstract class LegoCommunicationProtocol{ /** * Command types. Byte 0; @@ -68,33 +74,33 @@ public abstract class LegoCommunicationProtocol{ /** * Sensor types for setInputMode(). */ - public static final byte NO_SENSOR = 0x00; - public static final byte SWITCH = 0x01; - public static final byte TEMPERATURE = 0x02; - public static final byte REFLECTION = 0x03; - public static final byte ANGLE = 0x04; - public static final byte LIGHT_ACTIVE = 0x05; - public static final byte LIGHT_INACTIVE = 0x06; - public static final byte SOUND_DB = 0x07; - public static final byte SOUND_DBA = 0x08; - public static final byte CUSTOM = 0x09; - public static final byte LOWSPEED = 0x0A; - public static final byte LOWSPEED_9V = 0x0B; + public static final byte NO_SENSOR = 0x00; + public static final byte SWITCH = 0x01; + public static final byte TEMPERATURE = 0x02; + public static final byte REFLECTION = 0x03; + public static final byte ANGLE = 0x04; + public static final byte LIGHT_ACTIVE = 0x05; + public static final byte LIGHT_INACTIVE = 0x06; + public static final byte SOUND_DB = 0x07; + public static final byte SOUND_DBA = 0x08; + public static final byte CUSTOM = 0x09; + public static final byte LOWSPEED = 0x0A; + public static final byte LOWSPEED_9V = 0x0B; public static final byte NO_OF_SENSOR_TYPES = 0x0C; /** * Sensor modes for setInputMode(). */ - public static final byte RAWMODE = 0x00; - public static final byte BOOLEANMODE = 0x20; - public static final byte TRANSITIONCNTMODE = 0x40; - public static final byte PERIODCOUNTERMODE = 0x60; - public static final byte PCTFULLSCALEMODE = (byte)0x80; - public static final byte CELSIUSMODE = (byte)0xA0; - public static final byte FARENHEITMODE = (byte)0xC0; - public static final byte ANGLESTEPMODE = (byte)0xE0; - public static final byte SLOPEMASK = (byte)0x1F; - public static final byte MODEMASK = (byte)0xE0; + public static final byte RAWMODE = 0x00; + public static final byte BOOLEANMODE = 0x20; + public static final byte TRANSITIONCNTMODE = 0x40; + public static final byte PERIODCOUNTERMODE = 0x60; + public static final byte PCTFULLSCALEMODE = (byte)0x80; + public static final byte CELSIUSMODE = (byte)0xA0; + public static final byte FARENHEITMODE = (byte)0xC0; + public static final byte ANGLESTEPMODE = (byte)0xE0; + public static final byte SLOPEMASK = (byte)0x1F; + public static final byte MODEMASK = (byte)0xE0; /** * Firmware and protocol version request pdu. Page 11 of appendix 1. @@ -148,7 +154,7 @@ public abstract class LegoCommunicationProtocol{ if(turn_ratio < -100 || turn_ratio > 100){ throw new InvalidParameterException("Turn ratio out of range."); } - if(mode_byte != MOTORON && mode_byte != BRAKE && mode_byte != REGULATED){ + if(mode_byte < 0x00 || mode_byte > 0x07){ throw new InvalidParameterException("Invalid mode byte."); } if(regulation_mode != REGULATION_MODE_IDLE && regulation_mode != REGULATION_MODE_MOTOR_SPEED && regulation_mode != REGULATION_MODE_MOTOR_SYNC){ @@ -158,7 +164,7 @@ public abstract class LegoCommunicationProtocol{ throw new InvalidParameterException("Invalid run state."); } - message[0] = 0x0C; + message[0] = 0x0D; message[1] = 0x00; message[2] = DIRECT_COMMAND_NO_REPLY; message[3] = SET_OUTPUT_STATE; @@ -170,9 +176,26 @@ public abstract class LegoCommunicationProtocol{ message[9] = run_state; message[10] = message[11] = message[12] = message[13] = message[14] = 0x00; + Logger.log_d("LCP", LegoCommunicationProtocol.class.getSimpleName() + "setOutputState(...) :: " + Arrays.toString(message)); + return message; } + /** + *

Simpler call to the set motor configuration pdu method.

+ * + * @param output_port The port in the brick the motor is connected to. + * @param power Motor power. Must be between -100 and 100. + * @return The assembled pdu. + */ + public static byte[] setOutputState(byte output_port, byte power){ + if(power == (byte)0){ + return setOutputState(output_port, power, (byte)0x00, REGULATION_MODE_IDLE, (byte)0x00, MOTOR_RUN_STATE_IDLE); + }else{ + return setOutputState(output_port, power, (byte)(MOTORON + BRAKE), REGULATION_MODE_MOTOR_SPEED, (byte)0x00, MOTOR_RUN_STATE_RUNNING); + } + } + /** * Motor configuration request pdu. Page 8 of appendix 2. * diff --git a/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java new file mode 100644 index 0000000..2e77cce --- /dev/null +++ b/src/ve/ucv/ciens/ccg/nxtcam/robotcontrol/MotorEventQueue.java @@ -0,0 +1,61 @@ +package ve.ucv.ciens.ccg.nxtcam.robotcontrol; + +import java.util.LinkedList; +import java.util.Queue; + +import ve.ucv.ciens.ccg.networkdata.MotorEvent; + +/** + *

A simple monitor class that encapsulates a queue.

+ *

As it name says it stores motor events to be forwarded to the NXT robot.

+ *

This class implements the singleton design pattern.

+ * + * @author Miguel Angel Astor Romero + */ +public class MotorEventQueue { + /** + * The event queue implemented as a linked list. + */ + private Queue motorEvents; + + private MotorEventQueue(){ + motorEvents = new LinkedList(); + } + + private static class SingletonHolder{ + public static final MotorEventQueue instance = new MotorEventQueue(); + } + + /** + * Return the singleton instance of this class. + * @return The singleton instance. + */ + public static MotorEventQueue getInstance(){ + return SingletonHolder.instance; + } + + /** + *

Get the first event on the queue.

+ *

If there are no events to return this method blocks until some thread calls the addEvent() method.

+ * @return The event at the front of the queue. + */ + public synchronized MotorEvent getNextEvent(){ + while(motorEvents.size() == 0){ + try{ wait(); }catch(InterruptedException ie){ } + } + return motorEvents.poll(); + } + + /** + *

Adds an event to the back of the queue.

+ * @param event The event to add. + */ + public synchronized void addEvent(MotorEvent event){ + motorEvents.add(event); + notifyAll(); + } + + public synchronized int getSize(){ + return motorEvents.size(); + } +} diff --git a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java index 649bbd6..d86dd31 100644 --- a/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java +++ b/src/ve/ucv/ciens/ccg/nxtcam/utils/ProjectConstants.java @@ -21,9 +21,12 @@ import android.app.Activity; public abstract class ProjectConstants { // Network related constants. - public static final int SERVER_UDP_PORT = 8889; - public static final int SERVER_TCP_PORT_1 = 9989; - public static final int SERVER_TCP_PORT_2 = 9990; + public static final int SERVICE_DISCOVERY_PORT = 9988; + public static final int VIDEO_STREAMING_PORT = 9989; + public static final int MOTOR_CONTROL_PORT = 9990; + public static final int SENSOR_REPORT_PORT = 9991; + public static final int APP_CONTROL_PORT = 9992; + public static final UUID SERIAL_PORT_SERVICE_CLASS_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB"); public static final String OUI_LEGO = "00:16:53"; public static final String MULTICAST_ADDRESS = "230.0.0.1"; @@ -32,5 +35,5 @@ public abstract class ProjectConstants { // Activity results. public static final int RESULT_CAMERA_FAILURE = Activity.RESULT_FIRST_USER + 1; - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; }