-
Notifications
You must be signed in to change notification settings - Fork 1
/
NavigationCalibrationErrorTest.cxx
84 lines (66 loc) · 3.12 KB
/
NavigationCalibrationErrorTest.cxx
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
/*=========================================================================
Program: BRP Prostate Robot: Testing Simulator (Client)
Language: C++
Copyright (c) Brigham and Women's Hospital. All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. See the above copyright notices for more information.
Please see
http://wiki.na-mic.org/Wiki/index.php/ProstateBRP_OpenIGTLink_Communication_June_2013
for the detail of the testing protocol.
=========================================================================*/
#include <string.h>
#include "igtlOSUtil.h"
#include "igtlStringMessage.h"
#include "igtlClientSocket.h"
#include "igtlSocket.h"
#include "igtlStatusMessage.h"
#include "igtlTransformMessage.h"
#include "NavigationCalibrationErrorTest.h"
NavigationCalibrationErrorTest::NavigationCalibrationErrorTest()
{
}
NavigationCalibrationErrorTest::~NavigationCalibrationErrorTest()
{
}
NavigationCalibrationErrorTest::ErrorPointType NavigationCalibrationErrorTest::Test()
{
int queryCounter = 0;
igtl::MessageHeader::Pointer headerMsg;
headerMsg = igtl::MessageHeader::New();
std::cerr << "MESSAGE: ===== Step 1: START_UP =====" << std::endl;
SendStringMessage("CMD_0001", "START_UP");
ReceiveMessageHeader(headerMsg, this->TimeoutMedium);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0001", "START_UP")) return Error(1,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "START_UP")) return Error(1,2);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "START_UP", 1)) return Error(1,3);
std::cerr << "MESSAGE: ===== Step 2: PLANNING =====" << std::endl;
SendStringMessage("CMD_0002", "PLANNING");
ReceiveMessageHeader(headerMsg, this->TimeoutShort);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0002", "PLANNING")) return Error(2,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "PLANNING")) return Error(2,2);
std::cerr << "MESSAGE: ===== Step 3: CALIBRATION =====" << std::endl;
SendStringMessage("CMD_0003", "CALIBRATION");
ReceiveMessageHeader(headerMsg, this->TimeoutShort);
if (!CheckAndReceiveStringMessage(headerMsg, "ACK_0003", "CALIBRATION")) return Error(3,1);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CURRENT_STATUS", 1, 0, "CALIBRATION")) return Error(3,2);
// Create invalid matrix
igtl::Matrix4x4 matrix;
for (int i = 0; i < 4; i ++)
{
for (int j = 0; j < 4; j ++)
{
matrix[i][j] = 1.0;
}
}
SendTransformMessage("CLB_0004", matrix);
ReceiveMessageHeader(headerMsg, this->TimeoutShort);
if (!CheckAndReceiveTransformMessage(headerMsg, "ACK_0004", matrix)) return Error(3,3);
ReceiveMessageHeader(headerMsg, this->TimeoutLong);
if (!CheckAndReceiveStatusMessage(headerMsg, "CALIBRATION", igtl::StatusMessage::STATUS_CONFIG_ERROR)) return Error(3,4);
return SUCCESS;
}