KukieControlQueue::KukieControlQueue(double sleepTime, std::string deviceType, std::string armPrefix, ros::NodeHandle node, bool acceptCollisions) : ControlQueue(LBR_MNJ, sleepTime) {
commandTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/move";
retJointPosTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/get_state";
switchModeTopic = "/" + deviceType + "/" + armPrefix + "/settings/switch_mode";
retCartPosTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/get_pose_quat_wf";
stiffnessTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/set_impedance";
jntStiffnessTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/set_impedance";
ptpTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/ptp";
commandStateTopic = "/" + deviceType + "/" + armPrefix + "/settings/get_command_state";
ptpReachedTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/ptp_reached";
jntFrcTrqTopic = "/" + deviceType + "/" + armPrefix + "/sensoring/est_ext_jnt_trq";
cartFrcTrqTopic = "/" + deviceType + "/" + armPrefix + "/sensoring/cartesian_wrench";
cartPtpTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/ptpQuaternion";
cartPtpReachedTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/ptp_reached";
cartMoveRfQueueTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/move_rf";
cartMoveWfQueueTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/move_wf";
cartPoseRfTopic = "/" + deviceType + "/" + armPrefix + "/cartesian_control/get_pose_quat_rf";
jntSetPtpThreshTopic = "/" + deviceType + "/" + armPrefix + "/joint_control/set_ptp_thresh";
addLoadTopic = "not supported yet";
this->deviceType = deviceType;
this->armPrefix = armPrefix;
constructQueue(sleepTime, commandTopic, retJointPosTopic, switchModeTopic, retCartPosTopic, stiffnessTopic,
jntStiffnessTopic, ptpTopic, commandStateTopic, ptpReachedTopic, addLoadTopic, jntFrcTrqTopic, cartFrcTrqTopic,
cartPtpTopic, cartPtpReachedTopic, cartMoveRfQueueTopic, cartMoveWfQueueTopic, cartPoseRfTopic, jntSetPtpThreshTopic, acceptCollisions, node);
}
int runOS(OS* opSys, SimpleTimer* sysTime){
//variables
pcbQueue readyQ;
PCB running;
insQueue q;
char* elapTime;
char* outBuff;
float f = 0.0;
//constructions
constructQueue(&q);
constructPcbQueue(&readyQ, 10);
alloStr(&elapTime, 10);
alloStr(&outBuff, 50);
stop(sysTime);
getElapsedTime(&elapTime, sysTime);
f += atof(elapTime);
strcat(outBuff, elapTime);
strcat(outBuff, " - Simulator Program Starting");
outputHandler(opSys, outBuff);
start(sysTime);
//begin processing
processmdf(opSys, &readyQ);
stop(sysTime);
getElapsedTime(&elapTime, sysTime);
f += atof(elapTime);
start(sysTime);
heapsort(opSys, &readyQ);
while(pcbq_dequeue(&readyQ, &running))
{
runPCB(opSys, &running, &f);
}
stop(sysTime);
getElapsedTime(&elapTime, sysTime);
f += atof(elapTime);
clearStr(&outBuff);
strcat(outBuff, elapTime);
strcat(outBuff, " - Simulator Program Ending");
outputHandler(opSys, outBuff);
}
