// called at program finish
void inst_lptimer_fini() {
int i;
int e = 0;
// verify that loop entry counts == exit counts
for (i = 0; i < numberOfLoops; i++) {
if (entryCalled[i] != exitCalled[i]) {
PRINT_INSTR(stderr, "site %d: entry called %lld times but exit %lld", i, entryCalled[i], exitCalled[i]);
e++;
}
}
char filename[1024];
sprintf(filename, "meta_%04d.lptimerinst", getTaskId());
FILE * outfp = fopen(filename, "w");
write_lptimes(outfp);
fclose(outfp);
free(entryCalled);
free(exitCalled);
free(loopStatus);
free(loopTimes);
free(loopTimers);
if (e) {
exit(1);
}
}
void TwoWire::end() {
DTWI::I2C_STATUS status = di2c.getStatus();
if(beginCount == 0)
{
return;
}
destroyTask(getTaskId(onI2C));
beginCount = 0;
if(status.fMaster)
{
di2c.endMaster();
}
else if(status.fSlave)
{
// forcefully end the slave
di2c.endSlave(true);
}
// clean out the read / write buffer
di2c.abort();
di2c.discard();
}
int writeTask(FILE *outputFile, void *task){
int endedTasks, metaSize, taskState;
Task *auxTask = (Task *)task;
pthread_mutex_lock(&dependsOnMeMutex);
writeTaskIdList(outputFile, (void *)auxTask->dependsOnMe);
pthread_mutex_unlock(&dependsOnMeMutex);
writeTaskIdList(outputFile, auxTask->myDeps);
HashIntVoid *children = auxTask->children;
hashIntVoidSerialize(outputFile, children);
int id = getTaskId(auxTask);
WRITE_NUM(outputFile, "id", id);
endedTasks = getTaskEndedTasks(auxTask);
WRITE_NUM(outputFile, "endedTasks", endedTasks);
metaSize = getTaskMetasize(auxTask);
WRITE_NUM(outputFile, "metaSize", metaSize);
WRITE_BYTES(outputFile, getTaskMetadata(auxTask), metaSize);
taskState = getTaskState(auxTask);
WRITE_NUM(outputFile, "taskState", taskState);
writeDataSpace(outputFile, getTaskDataSpace(task));
return 1;
}
void SetGripperRotationPosition::stopExecution()
{
PlatformManipulatorAndIRBumper *platformManipulatorAndIRBumper = PlatformManipulatorAndIRBumper::getInstance();
platformManipulatorAndIRBumper->manGripperRotateStop();
stopped = true;
qDebug("Task#%lu (%s) stopExecution()", getTaskId(), getTaskName().c_str());
}
TaskScheduler::TaskID TaskScheduler::getTaskIdOrDie(const std::string& label)
{
auto id = getTaskId(label);
if (id == invalidTaskId)
die("No such task '%s'", label.c_str());
return id;
}
void TwoWire::begin(void)
{
if(beginCount == 0)
{
beginCount++;
// there are no callback in Master mode
destroyTask(getTaskId(onI2C));
di2c.beginMaster();
}
}
//--------------------------------------------------------------------------------
void BspProxyStubPool::takeYourPid(const int & taskPid){
int initialStackSize = lua_gettop(clientSideState);
lua_getglobal(clientSideState, getTaskId(taskPid).c_str());
lua_pushstring(clientSideState, "takeYourPid");
lua_gettable(clientSideState, -2);
lua_getglobal(clientSideState, getTaskId(taskPid).c_str());
lua_pushnumber(clientSideState, taskPid);
if (lua_pcall(clientSideState, 2, 0, 0) != 0){
cerr << "[ERROR] BspProxyStubPool::takeYourPid->Lua error: "
<< lua_tostring(clientSideState, -1) << endl;
lua_pop(clientSideState, 1);
}
lua_pop(clientSideState, 1);//removes proxy from stack
assert(initialStackSize == lua_gettop(clientSideState));
}
void HMMCalibrateParallelTask::prepare() {
TaskLocalData::createHMMContext(getTaskId(), false);
initTask = new HMMCreateWPoolTask(this);
addSubTask(initTask);
for(int i=0;i < settings.nThreads;i++){
addSubTask(new HMMCalibrateParallelSubTask(this));
}
setMaxParallelSubtasks(1);
}
// //--------------------------------------------------------------------------------
void BspProxyStubPool::registerRemoteIor(const string & ior, const int & processPid){
int initialStackSize = lua_gettop(clientSideState);
lua_getglobal(clientSideState, getTaskId(0).c_str());
lua_pushstring(clientSideState, "registerRemoteIor");
lua_gettable(clientSideState, -2);
lua_getglobal(clientSideState, getTaskId(0).c_str());
lua_pushstring(clientSideState, ior.c_str());
lua_pushnumber(clientSideState, processPid);
if (lua_pcall(clientSideState, 3, 0, 0) != 0){
cerr << "[ERROR] BspProxyStubPool::registerRemoteIor->Lua error: "
<< lua_tostring(clientSideState, -1) << endl;
lua_pop(clientSideState, 1);
}
lua_pop(clientSideState, 1);//removes proxy from stack
assert(initialStackSize == lua_gettop(clientSideState));
}
//--------------------------------------------------------------------------------
void BspProxyStubPool::bspSynch(const int & taskPid, int superstep){
int initialStackSize = lua_gettop(clientSideState);
lua_getglobal(clientSideState, getTaskId(0).c_str());
lua_pushstring(clientSideState, "bspSynch");
lua_gettable(clientSideState, -2);
lua_getglobal(clientSideState, getTaskId(0).c_str());
lua_pushnumber(clientSideState, taskPid);
lua_pushnumber(clientSideState, superstep);
if (lua_pcall(clientSideState, 3, 0, 0) != 0){
cerr << "[ERROR] BspProxyStubPool::bspSynch->Lua error: "
<< lua_tostring(clientSideState, -1) << endl;
lua_pop(clientSideState, 1);
}
lua_pop(clientSideState, 1);//removes proxy from stack
assert(initialStackSize == lua_gettop(clientSideState));
}
static bool beginSlave(void (*onReceiveService) (uint8_t*, int), void (*onRequestService)(void))
{
if(getTaskId(onI2C) == -1)
{
createTask(onI2C, 0, TASK_ENABLE, NULL);
};
onReceiveServiceR = onReceiveService;
onRequestServiceR = onRequestService;
iSessionCur = 0xFF;
return(true);
}
TaskScheduler::TaskID TaskScheduler::createTask(const std::string& label)
{
auto id = getTaskId(label);
if (id != invalidTaskId)
die("Task '%s' already exists", label.c_str());
id = tasks.size();
label2taskId[label] = id;
Task task {label, id};
tasks.push_back(task);
return id;
}
//--------------------------------------------------------------------------------
void BspProxyStubPool::bspSend(const int & taskPid, BspSend & bspSend){
int initialStackSize = lua_gettop(clientSideState);
lua_getglobal(clientSideState, getTaskId(taskPid).c_str());
lua_pushstring(clientSideState, "bspSend");
lua_gettable(clientSideState, -2);
lua_getglobal(clientSideState, getTaskId(taskPid).c_str());
lua_newtable(clientSideState);
LuaUtils::setFieldOnTable(clientSideState, "nBytes", bspSend.nBytes(), -1);
LuaUtils::setFieldOnTable(clientSideState, "tagSize", bspSend.tagSize(), -1);
LuaUtils::setFieldOnTable(clientSideState, "superstep", bspSend.superstep(), -1);
lua_pushstring(clientSideState, "payload");
const char * payload = (char *) bspSend.payload();
lua_pushlstring(clientSideState, payload, bspSend.nBytes());
lua_settable(clientSideState, -3);
lua_pushstring(clientSideState, "tag");
const char * tag = (char *) bspSend.tag();
lua_pushlstring(clientSideState, tag, bspSend.tagSize());
lua_settable(clientSideState, -3);
if (lua_pcall(clientSideState, 2, 0, 0) != 0){
cerr << "[ERROR] BspProxyStubPool::bspSend->Lua error: "
<< lua_tostring(clientSideState, -1) << endl;
lua_pop(clientSideState, 1);
}
lua_pop(clientSideState, 1);//removes proxy from stack
assert(initialStackSize == lua_gettop(clientSideState));
}
/// writeChildTask() writes only the necessary to
int writeChildTask(FILE *outputFile, void *task){
int metaSize = 0;
Task *auxTask = (Task *)task;
pthread_mutex_lock(&dependsOnMeMutex);
writeTaskIdList(outputFile, (void *)auxTask->dependsOnMe);
pthread_mutex_unlock(&dependsOnMeMutex);
writeTaskIdList(outputFile, auxTask->myDeps);
int id = getTaskId(auxTask);
WRITE_NUM(outputFile, "id", id);
metaSize = getTaskMetasize(auxTask);
WRITE_NUM(outputFile, "metaSize", metaSize);
WRITE_BYTES(outputFile, getTaskMetadata(auxTask), metaSize);
return 1;
}
void HMMCalibrateParallelTask::run() {
if (hasError() || isCanceled()) {
return;
}
TaskLocalData::bindToHMMContext(getTaskId());
try {
histogram_s* hist = getWorkPool()->hist;
if (!ExtremeValueFitHistogram(hist, TRUE, 9999.)) {
stateInfo.setError("fit failed; num sequences may be set too small?\n");
} else {
hmm->flags |= PLAN7_STATS;
hmm->mu = hist->param[EVD_MU];
hmm->lambda = hist->param[EVD_LAMBDA];
}
} catch (HMMException e) {
stateInfo.setError(e.error);
}
TaskLocalData::detachFromHMMContext();
}
void registerHandler(Messages::Type type, std::function<void(std::shared_ptr<Message>)> handler)
{
m_messageCommand[type] = [this, handler](ServerID_t serverId)
{
//std::chrono::time_point<std::chrono::high_resolution_clock, std::chrono::nanoseconds> now = std::chrono::high_resolution_clock::now();
//std::cout << "Received Message" << std::fixed << std::setprecision(10) << (now.time_since_epoch().count() / 1000000000.0) << std::endl;
auto message = std::make_shared<Message>();
Messages::read(*message, m_servers.get(serverId)->socket);
//
// Let the task queue know this task result has been recieved
if (TaskRequestQueue::instance()->finalizeTask(message->getTaskId(), true, true))
{
handler(message);
}
else
{
std::cout << "Not finalized" << std::endl;
}
};
}
bool SetGripperRotationPosition::checkFeasibility()
{
if (angle > 177 || angle < -89)
{
string msg = Valter::format_string("Task#%lu (%s) target angle %f in unreachable.", getTaskId(), getTaskName().c_str(), angle);
qDebug("%s", msg.c_str());
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~notes~%s", getTaskId(), msg.c_str()));
return false;
}
return true;
}
// called just after mpi_init
void* tool_mpi_init() {
nprocessors = sysconf(_SC_NPROCESSORS_ONLN);
pinto(getTaskId() % nprocessors);
}
bool SetGripperRotationPosition::initialize()
{
//script line parsing
std::vector<std::string> taskInitiationParts = Valter::split(taskScriptLine, '_');
std::string taskName = taskInitiationParts[0];
float angle = atof(((string)taskInitiationParts[1]).c_str());
setAngle(angle);
PlatformControlP1 *platformControlP1 = PlatformControlP1::getInstance();
if (!platformControlP1->getPower5VOnState())
{
string msg = Valter::format_string("Task#%lu (%s) could not be executed. 5V power is OFF", getTaskId(), getTaskName().c_str());
qDebug("%s", msg.c_str());
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~notes~%s", getTaskId(), msg.c_str()));
return false;
}
return true;
}
void SetGripperRotationPosition::execute()
{
if (initialize())
{
if (checkFeasibility())
{
new std::thread(&SetGripperRotationPosition::executionWorker, this);
this_thread::sleep_for(std::chrono::milliseconds(100));
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~%s~%s~%s~%s", getTaskId(), getTaskName().c_str(), (blocking) ? "blocking" : "non blocking", ((stopped) ? "stopped" : ((completed) ? "completed" : ((executing) ? "executing" : "queued"))), getTaskScriptLine().c_str()));
return;
}
}
this_thread::sleep_for(std::chrono::milliseconds(100));
stopExecution();
setCompleted();
}
Task::ReportResult HMMCalibrateParallelTask::report() {
TaskLocalData::freeHMMContext(getTaskId());
return ReportResult_Finished;
}
void SetGripperGrasperPosition::executionWorker()
{
qDebug("Task#%lu %s started", getTaskId(), taskName.c_str());
PlatformManipulatorAndIRBumper *platformManipulatorAndIRBumper = PlatformManipulatorAndIRBumper::getInstance();
/************************************ emulation *********************start***************************/
platformManipulatorAndIRBumper->setGripperADCPosition(300); //0 (opened) = 300;
/************************************ emulation *********************finish**************************/
float sigma = 0.25; //precision in mm
//opening - true
bool direction = (position > platformManipulatorAndIRBumper->getGripperPosition()) ? true : false;
float cutoffPosition = (direction) ? (position * 1) : (position / 1); //<<<<<<<<<<<<<<< dynamic parameter
if (abs(position - platformManipulatorAndIRBumper->getGripperPosition()) < sigma)
{
this_thread::sleep_for(std::chrono::milliseconds(100));
setCompleted();
return;
}
while (!stopped)
{
if (!executing)
{
if (direction) //open grasper
{
platformManipulatorAndIRBumper->manGripperOpen();
}
else
{
platformManipulatorAndIRBumper->manGripperClose();
}
executing = true;
}
else
{
if (qDebugOn)
{
qDebug("Task#%lu: GripperGrasperPosition (mm) = %f, target (mm) = %f, cutoffPosition = %f, dSigma = %f ? %f, direction: %s", getTaskId(), platformManipulatorAndIRBumper->getGripperPosition(), position, cutoffPosition, abs(position - platformManipulatorAndIRBumper->getGripperPosition()), sigma, (direction ? "openning" : "closing"));
}
if ((direction && platformManipulatorAndIRBumper->getGripperPosition() >= cutoffPosition) || ((!direction && platformManipulatorAndIRBumper->getGripperPosition() <= cutoffPosition)) || (abs(position - platformManipulatorAndIRBumper->getGripperPosition()) < sigma))
{
platformManipulatorAndIRBumper->manGripperStop();
setCompleted();
break;
}
/************************************ emulation *********************start***************************/
int positionADC = platformManipulatorAndIRBumper->getGripperADCPosition();
if (direction)
{
positionADC -= 1;
}
else
{
positionADC += 1;
}
platformManipulatorAndIRBumper->setGripperADCPosition(positionADC);
/************************************ emulation *********************finish**************************/
}
this_thread::sleep_for(std::chrono::milliseconds(50));
}
if (!getCompleted())
{
string msg = Valter::format_string("Task#%lu has been stopped via stopExecution() signal", getTaskId());
qDebug("%s", msg.c_str());
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~notes~%s", getTaskId(), msg.c_str()));
setCompleted();
}
}
void SetGripperRotationPosition::executionWorker()
{
qDebug("Task#%lu %s started", getTaskId(), taskName.c_str());
PlatformManipulatorAndIRBumper *platformManipulatorAndIRBumper = PlatformManipulatorAndIRBumper::getInstance();
/************************************ emulation *********************start***************************/
platformManipulatorAndIRBumper->setGripperADCRotation(355); //0 degrees = 355;
/************************************ emulation *********************finish**************************/
float sigma = 0.5; //precision in degrees
//true CW
bool direction = (angle > platformManipulatorAndIRBumper->getGripperRotation()) ? true : false;
float cutoffAngle = angle * 0.99; //<<<<<<<<<<<<<<< dynamic parameter
if (abs(abs(angle) - abs(platformManipulatorAndIRBumper->getGripperRotation())) < sigma)
{
this_thread::sleep_for(std::chrono::milliseconds(100));
setCompleted();
return;
}
while (!stopped)
{
if (!executing)
{
if (direction) //rotate gripper CW
{
platformManipulatorAndIRBumper->manGripperRotateCW();
}
else
{
platformManipulatorAndIRBumper->manGripperRotateCCW();
}
executing = true;
}
else
{
if (qDebugOn)
{
qDebug("Task#%lu: GripperRotation (deg) = %f, target (deg) = %f, cutoffAngle = %f, dSigma = %f ? %f, direction: %s", getTaskId(), platformManipulatorAndIRBumper->getGripperRotation(), angle, cutoffAngle, abs(abs(angle) - abs(platformManipulatorAndIRBumper->getGripperRotation())), sigma, (direction ? "CW" : "CCW"));
}
if ((direction && platformManipulatorAndIRBumper->getGripperRotation() >= cutoffAngle) || ((!direction && platformManipulatorAndIRBumper->getGripperRotation() <= cutoffAngle)) || (abs(angle - platformManipulatorAndIRBumper->getGripperRotation()) < sigma))
{
platformManipulatorAndIRBumper->manGripperRotateStop();
setCompleted();
break;
}
/************************************ emulation *********************start***************************/
int positionADC = platformManipulatorAndIRBumper->getGripperADCRotation();
if (direction)
{
positionADC += 1;
}
else
{
positionADC -= 1;
}
platformManipulatorAndIRBumper->setGripperADCRotation(positionADC);
/************************************ emulation *********************finish**************************/
}
this_thread::sleep_for(std::chrono::milliseconds(50));
}
if (!getCompleted())
{
string msg = Valter::format_string("Task#%lu has been stopped via stopExecution() signal", getTaskId());
qDebug("%s", msg.c_str());
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~notes~%s", getTaskId(), msg.c_str()));
setCompleted();
}
}
void SetGripperRotationPosition::reportCompletion()
{
qDebug("Task#%lu (%s) %s.", getTaskId(), getTaskName().c_str(), (stopped) ? "stopped" : "completed");
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~%s~%s~%s~%s", getTaskId(), getTaskName().c_str(), (blocking) ? "blocking" : "non blocking", ((stopped) ? "stopped" : ((completed) ? "completed" : ((executing) ? "executing" : "queued"))), getTaskScriptLine().c_str()));
}
bool SetGripperGrasperPosition::checkFeasibility()
{
if (position > 105 || position < 0)
{
string msg = Valter::format_string("Task#%lu (%s) target position %f in unreachable.", getTaskId(), getTaskName().c_str(), position);
qDebug("%s", msg.c_str());
TaskManager::getInstance()->sendMessageToCentralHostTaskManager(Valter::format_string("%lu~notes~%s", getTaskId(), msg.c_str()));
return false;
}
return true;
}
