|
|
|
|
|
|
|
|
#include "lfr_state_machine.h" |
|
|
#include "lfr_state_machine.h" |
|
|
|
|
|
|
|
|
LFR_StateMachine::LFR_StateMachine(LFR_IState& startState): |
|
|
|
|
|
|
|
|
// for string delimiter |
|
|
|
|
|
vector<string> LFR_StateMachine::split (string s, string delimiter) const { |
|
|
|
|
|
size_t pos_start = 0, pos_end, delim_len = delimiter.length(); |
|
|
|
|
|
string token; |
|
|
|
|
|
vector<string> res; |
|
|
|
|
|
|
|
|
|
|
|
while ((pos_end = s.find (delimiter, pos_start)) != string::npos) { |
|
|
|
|
|
token = s.substr (pos_start, pos_end - pos_start); |
|
|
|
|
|
pos_start = pos_end + delim_len; |
|
|
|
|
|
res.push_back (token); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
res.push_back (s.substr (pos_start)); |
|
|
|
|
|
return res; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_StateMachine::sanitize (string& s) const |
|
|
|
|
|
{ |
|
|
|
|
|
char r = '\r', n = '\n'; |
|
|
|
|
|
s.erase(std::remove(s.begin(), s.end(), n), s.end()); |
|
|
|
|
|
s.erase(std::remove(s.begin(), s.end(), r), s.end()); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
bool LFR_StateMachine::checkStringValidity (const std::vector<std::string>& s) const |
|
|
|
|
|
{ |
|
|
|
|
|
if(s.size() != 5) |
|
|
|
|
|
{ |
|
|
|
|
|
return false; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
try |
|
|
|
|
|
{ |
|
|
|
|
|
for(int i = 0; i < 4; i ++) |
|
|
|
|
|
{ |
|
|
|
|
|
if(stod(s[i]) > 1.0 || stod(s[i]) < -1.0) |
|
|
|
|
|
{ |
|
|
|
|
|
return false; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
int i = stoi(s[4]); |
|
|
|
|
|
if(i != 0 && i != 1) |
|
|
|
|
|
{ |
|
|
|
|
|
return false; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
|
|
|
catch(const std::exception& e) {return false;} |
|
|
|
|
|
return true; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_StateMachine::parseString(string s) |
|
|
|
|
|
{ |
|
|
|
|
|
sanitize(s); |
|
|
|
|
|
std::vector<std::string> splitStr = split(s, ";"); |
|
|
|
|
|
if(!checkStringValidity(splitStr)) |
|
|
|
|
|
{ |
|
|
|
|
|
std::cout<<"Invalid String" << std::endl; |
|
|
|
|
|
return; |
|
|
|
|
|
} |
|
|
|
|
|
double wheels[4] = {0.0, 0.0, 0.0, 0.0}; |
|
|
|
|
|
int mode = std::stoi(splitStr[4]); |
|
|
|
|
|
for(int i = 0; i < 3; i++) |
|
|
|
|
|
{ |
|
|
|
|
|
wheels[i] = std::stod(splitStr[i]); |
|
|
|
|
|
} |
|
|
|
|
|
if(mode == 0) { |
|
|
|
|
|
setState(State::Manual::getInstance()); |
|
|
|
|
|
uartCommunicator.sendTelegram(wheels[0], wheels[1], wheels[2], wheels[3]); |
|
|
|
|
|
} |
|
|
|
|
|
else if (mode == 1) {setState(State::Autonomous::getInstance());} |
|
|
|
|
|
return; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
LFR_StateMachine::LFR_StateMachine(): |
|
|
autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex) |
|
|
autonomousMode(videoHeight, videoWidth, thresholdBinary, gaussKernelSize, [&](std::exception const &ex) |
|
|
{ |
|
|
{ |
|
|
std::unique_lock<std::mutex> lock(mutex); |
|
|
std::unique_lock<std::mutex> lock(mutex); |
|
|
|
|
|
|
|
|
socket.addListener([&](LFR_Socket::LFR_Telegram telegram) |
|
|
socket.addListener([&](LFR_Socket::LFR_Telegram telegram) |
|
|
{ |
|
|
{ |
|
|
std::unique_lock<std::mutex> lock(mutex); |
|
|
std::unique_lock<std::mutex> lock(mutex); |
|
|
std::cout << telegram; |
|
|
|
|
|
|
|
|
parseString(std::string(telegram)); |
|
|
}, &mutex); |
|
|
}, &mutex); |
|
|
socket.startLoop(); |
|
|
socket.startLoop(); |
|
|
|
|
|
|
|
|
currentState = &startState; |
|
|
|
|
|
|
|
|
currentState = &State::Idle::getInstance(); |
|
|
currentState->enter(this); |
|
|
currentState->enter(this); |
|
|
|
|
|
|
|
|
//Start the permanent loop |
|
|
//Start the permanent loop |
|
|
|
|
|
|
|
|
std::cout << "binned" << std::endl; |
|
|
std::cout << "binned" << std::endl; |
|
|
} |
|
|
} |
|
|
std::cout << "Exiting central" << std::endl; |
|
|
std::cout << "Exiting central" << std::endl; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void LFR_StateMachine::setState(LFR_IState& newState) |
|
|
|
|
|
{ |
|
|
|
|
|
if (&newState != currentState) |
|
|
|
|
|
{ |
|
|
|
|
|
currentState->exit(this); |
|
|
|
|
|
currentState = &newState; |
|
|
|
|
|
currentState->enter(this); |
|
|
|
|
|
} |
|
|
|
|
|
else |
|
|
|
|
|
{ |
|
|
|
|
|
std::cout << "no switch" << std::endl; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
} |
|
|
} |