string smrclGenerator::initializeMission(vector<route> rPlan) {
string initStr;
const int STRLEN = 256;
char tempStr[STRLEN];
//Process the rules to generate command structure
clearCommands();
codeGen->processCommands(&cmds,false);
//Set condition value for first switch
initStr = "condVal = 1\n";
for(size_t i = 0; i < cmds.size(); i++) {
snprintf(tempStr,STRLEN," fail%d = 0\n",cmds[i].id);
initStr += tempStr;
}
//Put the next stage init commands in the success case
for (size_t i = 0; i < cmds.size(); i++) {
if ((cmds[i].quality > 0.0) && (!cmds[i].initCommand.empty())) {
initStr += cmds[i].initCommand;
initStr += "\n";
}
}
return initStr;
}
/**
* Omnibot destructor
*/
omnibot::~omnibot()
{
if(!_passedIrc)
{
delete _irc;
}
clearCommands();
delete _commands;
}
void plLogicModBase::read(hsStream* S, plResManager* mgr) {
plSingleModifier::read(S, mgr);
clearCommands();
fCommandList.setSizeNull(S->readInt());
for (size_t i=0; i<fCommandList.getSize(); i++)
fCommandList[i] = plMessage::Convert(mgr->ReadCreatable(S));
setNotify(plNotifyMsg::Convert(mgr->ReadCreatable(S)));
fLogicFlags.read(S);
fDisabled = S->readBool();
}
void plLogicModBase::IPrcParse(const pfPrcTag* tag, plResManager* mgr) {
if (tag->getName() == "LogicModParams") {
fDisabled = tag->getParam("Disabled", "false").toBool();
} else if (tag->getName() == "Commands") {
clearCommands();
fCommandList.setSizeNull(tag->countChildren());
const pfPrcTag* child = tag->getFirstChild();
for (size_t i=0; i<fCommandList.getSize(); i++) {
fCommandList[i] = plMessage::Convert(mgr->prcParseCreatable(child));
child = child->getNextSibling();
}
} else if (tag->getName() == "Notify") {
if (tag->hasChildren())
setNotify(plNotifyMsg::Convert(mgr->prcParseCreatable(tag->getFirstChild())));
} else if (tag->getName() == "LogicFlags") {
if (tag->hasChildren())
fLogicFlags.prcParse(tag->getFirstChild());
} else {
plSingleModifier::IPrcParse(tag, mgr);
}
}
void AbstractManager::clearCommands(int severity)
{
if(severity != -1)
{
if(mLock.tryLockForWrite(mWriteMaxWait))
{
CommandQueue* queue = mHashCommandQueues.value(severity,0);
if(queue)
{
qDeleteAll(queue->toSet());
queue->clear();
}
mLock.unlock();
}
}
else
{
foreach(int sever,mHashCommandQueues.keys())
{
clearCommands(sever);
}
}
}
void makeSimplePath(const char * s) {
int x; // a counter for the number of cells to be crossed
state_t state;
char c;
clearCommands();
state = PathStart;
x = 0;
while (state != PathExit) {
c = *s++;
switch (state) {
case PathStart:
if (c == 'F') {
x = 1;
state = PathOrtho_F;
} else if (c == 'R') {
emitCommand(CMD_ERROR_00);
state = PathStop;
} else if (c == 'L') {
emitCommand(CMD_ERROR_00);
state = PathStop;
} else if (c == 'S') {
state = PathStop;
} else {
emitCommand(CMD_ERROR_00);
state = PathStop;
}
break;
case PathOrtho_F:
if (c == 'F') {
x++;
} else if (c == 'R') {
emitCommand(FWD0 + x);
state = PathOrtho_R;
} else if (c == 'L') {
emitCommand(FWD0 + x);
state = PathOrtho_L;
} else if (c == 'S') {
emitCommand(FWD0 + x);
state = PathStop;
} else {
emitCommand(CMD_ERROR_01);
state = PathStop;
}
break;
case PathOrtho_R:
if (c == 'F') {
emitCommand(SS90ER);
x = 2;
state = PathOrtho_F;
} else if (c == 'R') {
emitCommand(SS90ER);
emitCommand(FWD1);
state = PathOrtho_R;
} else if (c == 'L') {
emitCommand(SS90ER);
emitCommand(FWD1);
state = PathOrtho_L;
} else if (c == 'S') {
emitCommand(SS90ER);
emitCommand(FWD1);
state = PathStop;
} else {
emitCommand(CMD_ERROR_02);
state = PathStop;
}
break;
case PathOrtho_L:
if (c == 'F') {
emitCommand(SS90EL);
x = 2;
state = PathOrtho_F;
} else if (c == 'R') {
emitCommand(SS90EL);
emitCommand(FWD1);
state = PathOrtho_R;
} else if (c == 'L') {
emitCommand(SS90EL);
emitCommand(FWD1);
state = PathOrtho_L;
} else if (c == 'S') {
emitCommand(SS90EL);
emitCommand(FWD1);
state = PathStop;
} else {
emitCommand(CMD_ERROR_03);
state = PathStop;
}
break;
case PathStop:
emitCommand(CMD_STOP); // make sure the command list gets terminated
state = PathExit;
break;
default:
emitCommand(CMD_ERROR_15);
state = PathExit;
break;
}
}
}
ConsoleInput::~ConsoleInput() {
clearPathsToFiles();
clearCommands();
//std::cout << "ConsoleInput::~ConsoleInput()" << std::endl;
}
/** Robot code assembler
*
* This function generates a switch statement for continous robot movement
*
*/
string smrclGenerator::generateSwitch(vector<route> rPlan,size_t pIndex, bool wait) {
string cmdStr;
const int STRLEN = 256;
char tempStr[STRLEN];
int caseCnt = 1;
//Error check
if (rPlan.size() <= (size_t)pIndex) {
printf("Generator: Plan is has less elements than index");
return cmdStr;
}
//Save the old commands (currently not used)
oldCmds = cmds;
//Clear the commands vector
clearCommands();
//Process the rules to generate command structure
codeGen->processCommands(&cmds,wait);
codeGen->processNextCommands(&nextCmds,wait);
//Set the id's
crashId = cmds.size() + 1;
successId = cmds.size() + 2;
//Sort the commands in highest quality first order
sort(cmds.begin(),cmds.end());
reverse(cmds.begin(),cmds.end());
sort(nextCmds.begin(),nextCmds.end());
reverse(nextCmds.begin(),nextCmds.end());
//Generate commands output for the route element
//cmds = processCommandsFromRule();//processCommands(rPlan,pIndex);//
//Generate commands output for the next route element (forward planning)
//(Only if the route has enough elements)
// nextCmds = processCommands(rPlan,pIndex+1);
//Reset trigger variables
cmdStr += "success = 0\n";
cmdStr += "reload = 0\n";
//Start the switch
cmdStr += "switch (condVal)\n";
//Make a case of the highest quality command for the following route element
//to execute, if the previous command is finished successfully
snprintf(tempStr,STRLEN," putevent \"ev%d\"\n",successId);
cmdStr += tempStr;
//Clear the fail-variables
for(size_t i = 0; i < cmds.size(); i++) {
snprintf(tempStr,STRLEN," fail%d = 0\n",cmds[i].id);
cmdStr += tempStr;
}
//Load the init variables
for (size_t i = 0; i < nextCmds.size(); i++)
if ((nextCmds[i].quality > 0) && (!nextCmds[i].initCommand.empty())) {
cmdStr += nextCmds[i].initCommand;
cmdStr += "\n";
}
//Put the next command if there is any elements left in the mission, otherwise stop
if ((pIndex+1) >= rPlan.size()) {
cmdStr += "stop\n wait 2\n idle\n"; //Stop the robot
} else {
cmdStr += makeCaseCommand(nextCmds, 0);
}
//Add the case-commands for the command, if the command is not empty
for(size_t i = 0; i < cmds.size(); i++) {
if (!cmds[i].command.empty()) {
snprintf(tempStr,STRLEN," case %d\n",caseCnt);
caseCnt++;
cmdStr += tempStr;
snprintf(tempStr,STRLEN," putevent \"ev%d\"\n",cmds[i].id);
cmdStr += tempStr;
//Add the command itself
cmdStr += makeCaseCommand(cmds, i);
}
}
//Make a "last resort case"
snprintf(tempStr,STRLEN," case %d\n",caseCnt);
caseCnt++;
cmdStr += tempStr;
snprintf(tempStr,STRLEN," putevent \"ev%d\"\n",crashId);
cmdStr += tempStr;
snprintf(tempStr,STRLEN," stop\n");
cmdStr += tempStr;
//Make a case of the highest quality command for the following route element
//to execute, if the previous command is finished successfully
snprintf(tempStr,STRLEN," case %d\n",successId);
caseCnt++;
cmdStr += tempStr;
snprintf(tempStr,STRLEN," putevent \"ev%d\"\n",successId);
cmdStr += tempStr;
for(size_t i = 0; i < cmds.size(); i++) {
snprintf(tempStr,STRLEN," fail%d = 0\n",cmds[i].id);
cmdStr += tempStr;
}
for (size_t i = 0; i < nextCmds.size(); i++) {
if ((cmds[i].quality > 0) && (!cmds[i].exitCommand.empty())) {
cmdStr += cmds[i].exitCommand;
cmdStr += "\n";
}
if ((nextCmds[i].quality > 0) && (!nextCmds[i].initCommand.empty())) {
cmdStr += nextCmds[i].initCommand;
cmdStr += "\n";
}
}
//Put the next command if there is any elements left in the mission, otherwise stop
if ((pIndex+1) >= rPlan.size()) {
cmdStr += "stop\n wait 2\n idle\n"; //Stop the robot
} else {
cmdStr += makeCaseCommand(nextCmds, 0);
}
//Finally round up the switch
cmdStr += "endswitch\n";
return cmdStr;
}
