-
Notifications
You must be signed in to change notification settings - Fork 0
/
ProstateRobotStartUpPhase.cpp
44 lines (39 loc) · 1.18 KB
/
ProstateRobotStartUpPhase.cpp
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
#include "ProstateRobotStartUpPhase.hpp"
ProstateRobotStartUpPhase::ProstateRobotStartUpPhase() : ProstateRobotPhaseBase()
{
forbidden_transition_list = {
kStateNames.START_UP,
kStateNames.UNDEFINED,
kStateNames.PLANNING,
kStateNames.TARGETING,
kStateNames.MOVE_TO_TARGET
};
}
ProstateRobotStartUpPhase::~ProstateRobotStartUpPhase()
{
}
void ProstateRobotStartUpPhase::OnExit()
{
}
int ProstateRobotStartUpPhase::Initialize()
{
this->SendStatusMessage(this->Name(), igtl::StatusMessage::STATUS_OK, 0);
return 1;
}
int ProstateRobotStartUpPhase::MessageHandler(igtl::MessageHeader *headerMsg)
{
// Receive and set needle length
if (strcmp(headerMsg->GetDeviceType(), kMsgTypes.STRING.c_str()) == 0 &&
strcmp(headerMsg->GetDeviceName(), kMsgBody.NEEDLE_LENGTH.c_str()) == 0)
{
// Send acknowledgment for retract needle command
string needle_length;
ReceiveString(headerMsg, needle_length);
this->RStatus->SetNeedleLength(std::stod(needle_length));
std::stringstream ss;
ss << kCommunicationCmds.ACKNOWLEDGE << kMsgBody.NEEDLE_LENGTH;
SendStringMessage(ss.str().c_str(), needle_length.c_str());
return 1;
}
return 0;
}