-
Notifications
You must be signed in to change notification settings - Fork 0
/
ProstateRobotStopPhase.cpp
49 lines (43 loc) · 1.46 KB
/
ProstateRobotStopPhase.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
45
46
47
48
49
#include "ProstateRobotStopPhase.hpp"
ProstateRobotStopPhase::ProstateRobotStopPhase() : ProstateRobotPhaseBase()
{
// Don't let transition to states that involve robot motion unless the transition criteria in isTransitionAllowed function is satisfied.
forbidden_transition_list = {
kStateNames.STOP,
kStateNames.UNDEFINED,
kStateNames.TARGETING,
kStateNames.MOVE_TO_TARGET
};
}
ProstateRobotStopPhase::~ProstateRobotStopPhase()
{
}
void ProstateRobotStopPhase::OnExit()
{
}
int ProstateRobotStopPhase::Initialize()
{
this->SendStatusMessage(this->Name(), igtl::StatusMessage::STATUS_OK, 0);
// Stop all motors and disable them.
robot->StopRobot();
robot->motion_controller_.DisableAllMotors();
return 1;
}
int ProstateRobotStopPhase::MessageHandler(igtl::MessageHeader *headerMsg)
{
return 0;
}
bool ProstateRobotStopPhase::IsTransitionAllowed(const std::string &desired_next_workphase)
{
// If transitioned from move_to_target and transitioning back to move_to_target allow the transition.
if (GetPreviousWorkPhase() == kStateNames.MOVE_TO_TARGET && desired_next_workphase == kStateNames.MOVE_TO_TARGET)
{
return true;
}
// If transitioned from targeting and transitioning back to targeting allow the transition.
else if (GetPreviousWorkPhase() == kStateNames.TARGETING && desired_next_workphase == kStateNames.TARGETING)
{
return true;
}
return ProstateRobotPhaseBase::IsTransitionAllowed(desired_next_workphase);
}