private bool ReceiveResponse(out byte[] payload, out byte checkSum) { payload = new byte[0]; checkSum = 0; byte[] receiveHeader = new byte[8]; if (streamControl.Read(receiveHeader, 0, receiveHeader.Length) == 0) { log.Error("Got no response from camera"); return(false); } // first 4 bytes must be STX for (int i = 0; i < START_STX.Length; i++) { if (START_STX[i] != receiveHeader[i]) { log.Error($"Response did not start with {nameof(START_STX)}."); return(false); } } Int32 payloadLength = Utils.FromBigEndianInt32(receiveHeader, 4); byte[] receivePayload = new byte[payloadLength + 1]; // 1 Byte for checksum streamControl.Read(receivePayload, 0, receivePayload.Length); payload = new byte[payloadLength]; Array.Copy(receivePayload, payload, payloadLength); checkSum = receivePayload[receivePayload.Length - 1]; log.DebugFormat("Received paylod: {0}", Encoding.ASCII.GetString(payload)); return(true); }
/// <summary> /// Instantiates a camera object of a given type (identified by its type name). /// </summary> /// <param name="name">The type name (the output of typeof([CameraClassName]).ToString()).</param> /// <param name="dllPath">Returns the path of the DLL which contains the camera type.</param> /// <returns>A new instance of the camera class.</returns> public static Camera GetCameraInstanceByName(string name, out string dllPath) { log.DebugFormat("GetCameraInstanceByName({0}, out)", name); Type cameraType; try { cameraType = loadedCameraTypes[name]; dllPath = loadedCameraTypesDLLPaths[name]; } catch (KeyNotFoundException) { string name2 = EnsureMetriCam2CameraPrefix(name); log.DebugFormat(" camera with name {0} not found, trying {1}", name, name2); try { cameraType = loadedCameraTypes[name2]; dllPath = loadedCameraTypesDLLPaths[name2]; } catch (KeyNotFoundException) { string msg = string.Format("The camera type \"{0}\" is not registered. Please add the correct DLL.", name); log.Warn(msg); throw new ArgumentException(msg); } } ConstructorInfo constructor; try { constructor = cameraType.GetConstructor(new Type[] { }); if (null == constructor) { throw new Exception(); } } catch (Exception) { string msg = string.Format("The default constructor of the camera type \"{0}\" could not be found (probably missing).", name); log.Error(msg); throw new ArgumentException(msg); } Camera cam; try { cam = (Camera)constructor.Invoke(null); } catch (Exception ex) { string msg = string.Format("The default constructor of the camera type \"{0}\" threw an exception: {1}", name, ex.Message); log.Error(msg); throw new Exception(msg); } log.DebugFormat(" camera {0} found", cam.Name); return(cam); }
/// <summary> /// Parses the binary data. /// </summary> private void Parse() { int offset = 0; // 2 bytes: blob id ushort blobId = Utils.FromBigEndianUInt16(ImageBuffer, offset); if (0x0001 != blobId) { string msg = string.Format("{0}: The blob id is not 0x0001 as expected: {1:X4}", cam.Name, blobId); log.Error(msg); throw new InvalidDataException(msg); } offset += 2; // 2 bytes: number of segments ushort numSegments = Utils.FromBigEndianUInt16(ImageBuffer, offset); if (numSegments != 3) { string msg = string.Format("{0}: The number of segments is not 3 as expected: {1}", cam.Name, numSegments); log.Error(msg); throw new InvalidDataException(msg); } offset += 2; // Next 8 * numSegments bytes: Offset and change counter for each segment uint[] offsets = new uint[numSegments]; uint[] changedCounters = new uint[numSegments]; for (int i = 0; i < numSegments; ++i) { offsets[i] = Utils.FromBigEndianUInt32(ImageBuffer, offset); offset += 4; changedCounters[i] = Utils.FromBigEndianUInt32(ImageBuffer, offset); offset += 4; } // now: XML segment string xml = Encoding.ASCII.GetString(ImageBuffer, (int)offsets[0], (int)offsets[1] - (int)offsets[0]); ParseXML(xml, out int numBytesPerIntensityValue, out int numBytesPerDistanceValue, out int numBytesPerConfidenceValue); // calc sizes int numBytesIntensity = Width * Height * numBytesPerIntensityValue; int numBytesDistance = Width * Height * numBytesPerDistanceValue; int numBytesConfidence = Width * Height * numBytesPerConfidenceValue; // now: save image data offsets uint binarySegmentSize = offsets[2] - offsets[1]; ParseBinary((int)offsets[1], binarySegmentSize, numBytesIntensity, numBytesDistance, numBytesConfidence); }
/// <summary> /// Gets the raw frame data from camera. /// </summary> /// <remarks>Data is checked for correct protocol version and packet type.</remarks> /// <returns>Raw frame</returns> internal byte[] GetFrameData() { if (!Utils.SyncCoLa(streamData)) { string msg = string.Format("{0}: Could not sync to CoLa bus", cam.Name); log.Error(msg); throw new IOException(msg); } log.Debug("Start getting frame"); byte[] buffer = new byte[0]; if (!Utils.Receive(streamData, ref buffer, 4)) { string msg = string.Format("{0}: Could not read package length", cam.Name); log.Error(msg); throw new IOException(msg); } uint pkgLength = Utils.FromBigEndianUInt32(buffer, 0); if (!Utils.Receive(streamData, ref buffer, (int)pkgLength)) { string msg = string.Format("{0}: Could not read package payload", cam.Name); log.Error(msg); throw new IOException(msg); } // check buffer content int offset = 0; ushort protocolVersion = Utils.FromBigEndianUInt16(buffer, offset); offset += 2; byte packetType = buffer[offset]; offset += 1; if (0x0001 != protocolVersion) { string msg = string.Format("{0}: The protocol version is not 0x0001 as expected: {1:X4}", cam.Name, protocolVersion); log.Error(msg); throw new InvalidDataException(msg); } if (0x62 != packetType) { string msg = string.Format("{0}: The packet type is not 0x62 as expected: {1:X2}", cam.Name, packetType); log.Error(msg); throw new InvalidDataException(msg); } return(buffer.Skip(offset).ToArray()); }
public Control(MetriLog log, string ipAddress) { this.log = log; try { sockControl = new TcpClient(ipAddress, TCP_PORT_SOPAS); streamControl = sockControl.GetStream(); } catch (Exception ex) { string msg = string.Format("Failed to connect to IP={0}, reasons={1}", ipAddress, ex.Message); log.Error(msg); throw new Exceptions.ConnectionFailedException(msg, ex); } _accessMode = GetAccessMode(); InitStream(); }
/// <summary> /// Tells the device that there is a streaming channel. /// </summary> public void Control_InitStream() { log.Debug("Initializing streaming"); byte[] toSend = AddFraming("sMN GetBlobClientConfig"); byte[] receive = new byte[50]; // send ctrl message streamControl.Write(toSend, 0, toSend.Length); // get response if (streamControl.Read(receive, 0, receive.Length) == 0) { log.Error("Got no answer from camera"); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_setParameter", "Failed to init stream."); } else { string response = Encoding.ASCII.GetString(receive); log.DebugFormat("Got response: {0}", response); } log.Debug("Done: Initializing streaming"); }
/// <summary> /// Creates a new Device instance which can be used to handle the low level TCP communication /// between camera and client. /// </summary> /// <param name="ipAddress">IP address of client</param> /// <param name="cam">MetriCam2 camera object used for exceptions</param> /// <param name="log">MetriLog</param> internal Device(string ipAddress, VisionaryT cam, MetriLog log) { this.cam = cam; this.log = log; try { sockData = new TcpClient(ipAddress, TCP_PORT_BLOBSERVER); } catch (Exception ex) { string msg = string.Format("{0}: Failed to connect to IP {1}{2}Reason: {3}", cam.Name, ipAddress, Environment.NewLine, ex.Message); log.Error(msg); throw new Exceptions.ConnectionFailedException(msg, ex); } streamData = sockData.GetStream(); // say "hello" to camera byte[] hbBytes = Encoding.ASCII.GetBytes(HEARTBEAT_MSG); streamData.Write(hbBytes, 0, hbBytes.Length); }
static void Main(string[] args) { MetriLog log = new MetriLog(); Kinect2 cam = new Kinect2(); try { cam.Connect(); } catch (MetriCam2.Exceptions.ConnectionFailedException) { log.Error("Connection failed. Closing window in 5 sec."); Thread.Sleep(5 * 1000); return; } cam.ActivateChannel(ChannelNames.Color); bool running = false; while (running) { cam.Update(); } ProjectiveTransformationZhang pt; try { pt = (ProjectiveTransformationZhang)cam.GetIntrinsics(ChannelNames.Color); } catch (FileNotFoundException) { log.Warn("No PT found."); } try { pt = (ProjectiveTransformationZhang)cam.GetIntrinsics(ChannelNames.Color); } catch (FileNotFoundException) { log.Warn("No PT found."); } try { pt = (ProjectiveTransformationZhang)cam.GetIntrinsics(ChannelNames.Color); } catch (FileNotFoundException) { log.Warn("No PT found."); } try { RigidBodyTransformation rbt = cam.GetExtrinsics(ChannelNames.Color, ChannelNames.ZImage); } catch (FileNotFoundException) { log.Warn("No fwd RBT found."); } try { RigidBodyTransformation rbtInverse = cam.GetExtrinsics(ChannelNames.ZImage, ChannelNames.Color); } catch (FileNotFoundException) { log.Warn("No inverse RBT found."); } cam.Disconnect(); log.Info("Program ended. Closing window in 5 sec."); Thread.Sleep(5 * 1000); }
/// <summary> /// This methods parses the data provided by constructor and sets the properties accordingly. /// </summary> public void Read() { // first 11 bytes: internal definitions consisting of: // 4 bytes STx magicWord = BitConverter.ToUInt32(imageData, 0); if (0x02020202 != magicWord) { log.Error("The framing header is not 0x02020202 as expected."); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_unknown", "The framing header does not have the expected value."); } magicWord = Utils.ConvertEndiannessUInt32(magicWord); // 4 bytes packet length pkgLength = BitConverter.ToUInt32(imageData, 4); pkgLength = Utils.ConvertEndiannessUInt32(pkgLength); // 2 bytes protocol version protocolVersion = BitConverter.ToUInt16(imageData, 8); protocolVersion = Utils.ConvertEndiannessUInt16(protocolVersion); if (0x0001 != protocolVersion) { log.Error("The protocol version is not 0x0001 as expected."); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_unknown", "Unexpected protocol version"); } // 1 byte packet type packetType = imageData[10]; if (0x62 != packetType) { log.Error("The packet type is not 0x62 as expected."); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_unknown", "Unexpected packet type"); } UInt16 blobId = BitConverter.ToUInt16(imageData, 11); if (0x0001 == blobId) { log.Error("The blob id is not 0x0001 as expected."); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_unknown", "Unexpected blob id"); } blobId = Utils.ConvertEndiannessUInt16(blobId); // Next 4 bytes: blob id and number of segments numSegments = BitConverter.ToUInt16(imageData, 13); numSegments = Utils.ConvertEndiannessUInt16(numSegments); if (numSegments != 3) { log.Error("Number of segments is not three."); ExceptionBuilder.Throw(typeof(InvalidOperationException), cam, "error_unknown", "No segments found."); } // Next 8 * numSegments bytes: Offset and change counter for each segment offsets = new uint[numSegments]; changedCounters = new uint[numSegments]; for (int i = 0; i < numSegments; ++i) { int index = i * 8 + 15; // 8 per item + 15 is offset offsets[i] = BitConverter.ToUInt32(imageData, index); changedCounters[i] = BitConverter.ToUInt32(imageData, index + 4); offsets[i] = Utils.ConvertEndiannessUInt32(offsets[i]); changedCounters[i] = Utils.ConvertEndiannessUInt32(changedCounters[i]); // First internal defintions took up 11 bytes offsets[i] += 11; } // now: XML segment string xml = Encoding.ASCII.GetString(imageData, (int)offsets[0], (int)offsets[1] - (int)offsets[0]); ReadXML(xml); // calc sizes numBytesIntensity = width * height * numBytesPerIntensityValue; numBytesDistance = width * height * numBytesPerDistanceValue; numBytesConfidence = width * height * numBytesPerConfidenceValue; // now: save image data offsets ReadBinary(); }