void SetGripperRotationPosition::stopExecution()
{
PlatformManipulatorAndIRBumper *platformManipulatorAndIRBumper = PlatformManipulatorAndIRBumper::getInstance();
platformManipulatorAndIRBumper->manGripperRotateStop();
stopped = true;
qDebug("Task#%lu (%s) stopExecution()", getTaskId(), getTaskName().c_str());
}
BSONObj _omTaskStrategyInfo::toBSON() const
{
BSONObjBuilder builder( 1024 ) ;
builder.append( OM_REST_FIELD_RULE_ID, getID() ) ;
builder.append( OM_REST_FIELD_TASK_ID, getTaskID() ) ;
builder.append( OM_REST_FIELD_TASK_NAME, getTaskName() ) ;
builder.append( OM_REST_FIELD_NICE, getNice() ) ;
builder.append( OM_REST_FIELD_USER_NAME, getUserName() ) ;
BSONArrayBuilder arr( builder.subarrayStart( OM_REST_FIELD_IPS ) ) ;
SET_IP::const_iterator cit = _ips.begin() ;
while( cit != _ips.end() )
{
if ( !cit->empty() )
{
arr.append( *cit ) ;
}
++cit ;
}
arr.done() ;
return builder.obj() ;
}
bool PlanEntryModel::operator ==(PlanEntryModel &tem)
{
bool b = true;
if(compareTo(&tem) != 0)b = false;
else if(tem.getTaskID() != getTaskID())b = false;
else if(tem.getTaskStart() != getTaskStart())b = false;
else if(tem.getTaskStop() != getTaskStop())b = false;
else if(tem.getPercentComplete() != getPercentComplete())b = false;
else if(_tcscmp(tem.GetOwner(), GetOwner()) != 0)b = false;
else if(_tcscmp(tem.getTaskName(), getTaskName()) != 0)b = false;
else if(_tcscmp(tem.getAnnotation(), getAnnotation()) != 0)b = false;
else if(GetEntryType() != tem.GetEntryType())b = false;
return b;
}
void DependentTaskBase::setDeployment(const std::shared_ptr< orocos_cpp::Deployment >& newDeployment)
{
if(!prefix.empty())
throw std::runtime_error("init::DependentTaskBase::setDeployment: error, setting deployment, after setting of prefix is not allowed");
if(!newDeployment)
throw std::runtime_error("init::DependentTaskBase::setDeployment: Error, deployment contains nullptr");
deployment = newDeployment;
//check if our task name is present in the deployment
const auto taskNames = deployment->getTaskNames();
if(std::find(taskNames.begin(), taskNames.end(), getTaskName()) == taskNames.end())
throw std::runtime_error("init::DependentTaskBase::setDeployment: error, task name " + getTaskName() + " was not found in deployment " + deployment->getName());
}
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()));
}
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();
}
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;
}
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;
}
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;
}
