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