void TrajectoryThread::ct_run()
{
if (goalAttained() || deactivationLatch)
{
switch (terminationStrategy)
{
case BACK_AND_FORTH:
flipWaypoints();
setTrajectoryWaypoints(allWaypoints, true);
break;
case STOP_THREAD:
stop();
break;
case STOP_THREAD_DEACTIVATE:
if(deactivateTask()){
stop();
}else{
std::cout << "[WARNING] Trajectory id = "<< ControlThread::threadId << " for task: " << originalTaskParams.name << " has attained its goal state, but cannot be deactivated." << std::endl;
yarp::os::Time::delay(1.0); // try again in one second.
deactivationDelay += 1.0;
if(deactivationDelay >= deactivationTimeout){
std::cout << "[WARNING] Deactivation timeout." << std::endl;
stop();
}
}
break;
case WAIT:
if (printWaitingNoticeOnce) {
std::cout << "Trajectory id = "<< ControlThread::threadId << " for task: " << originalTaskParams.name << " has attained its goal state. Awaiting new commands." << std::endl;
printWaitingNoticeOnce = false;
}
break;
case WAIT_DEACTIVATE:
if (printWaitingNoticeOnce) {
if(deactivateTask()){
std::cout << "Trajectory id = "<< ControlThread::threadId << " for task: " << originalTaskParams.name << " has attained its goal state. Deactivating task and awaiting new commands." << std::endl;
printWaitingNoticeOnce = false;
deactivationLatch = true;
}else{
std::cout << "Trajectory id = "<< ControlThread::threadId << " for task: " << originalTaskParams.name << " has attained its goal state and is awaiting new commands. [WARNING] Could not deactivate the task." << std::endl;
yarp::os::Time::delay(1.0); // try again in one second.
deactivationDelay += 1.0;
if(deactivationDelay >= deactivationTimeout){
printWaitingNoticeOnce = false;
std::cout << "[WARNING] Deactivation timeout." << std::endl;
}
}
}
break;
}
}
else{
if (!currentTaskParams.isActive) {
activateTask();
}
desStateBottle.clear();
if (trajType==GAUSSIAN_PROCESS)
{
Eigen::MatrixXd desiredState_tmp;
trajectory->getDesiredValues(yarp::os::Time::now(), desiredState_tmp, desiredVariance);
desiredState << desiredState_tmp;
for(int i=0; i<desiredState.size(); i++)
{
desStateBottle.addDouble(desiredState(i));
}
#if USING_SMLT
if(useVarianceModulation)
{
Eigen::VectorXd desiredWeights = varianceToWeights(desiredVariance);
for(int i=0; i<desiredWeights.size(); i++)
{
desStateBottle.addDouble(desiredWeights(i));
}
}
#endif
}
else
{
desiredState << trajectory->getDesiredValues(yarp::os::Time::now());
for(int i=0; i<desiredState.size(); i++)
{
desStateBottle.addDouble(desiredState(i));
}
}
outputPort.write(desStateBottle);
}
}
/**
* Assumes that the operation prior set the state to whatever is current.
*
* @return true if the state is stable, and the integrator should be notified.
*/
bool LSM9DSx_Common::step_state() {
if (!desired_state_attained()) {
if (error_condition) {
// We shouldn't be changing states if there is an error condition.
// Reset is the only way to exit the condition at present.
if (getVerbosity() > 2) {
local_log.concatf("%s step_state() was called while we are in an error condition: %s\n", imu_type(), getErrorString());
Kernel::log(&local_log);
}
return true;
}
switch (getState()) {
case State::STAGE_0: // We think the IIU might be physicaly absent.
//reset(); ?
identity_check();
break;
case State::STAGE_1: // We are sure the IMU is present, but we haven't done anything with it.
configure_sensor();
break;
case State::STAGE_2: // Discovered and initiallized, but unknown register values.
if (is_setup_completed()) {
bulk_refresh();
}
else {
set_state(State::STAGE_1);
return true;
}
break;
case State::STAGE_3: // Fully initialized and sync'd. Un-calibrated.
integrator->state_change_notice(this, State::STAGE_3, State::STAGE_3); // TODO: Wrong.
sb_next_write = 0;
readSensor();
break; // TODO: Stop skipping calibrate().
case State::STAGE_4: // Calibrated and idle.
if (desiredState() == State::STAGE_5) {
// Enable the chained reads, and start the process rolling.
readSensor();
}
else {
// Downgrading to init state 3 (recalibrate).
set_state(State::STAGE_3);
sb_next_write = 0;
readSensor();
return true;
}
break;
case State::STAGE_5: // Calibrated and reading.
switch (desiredState()) {
case State::STAGE_4: // Stop reads.
set_state(State::STAGE_4);
return false; /// Note the slight break from convention... Careful...
case State::STAGE_3: // Downgrading to init state 3 (recalibrate).
set_state(State::STAGE_3);
sb_next_write = 0;
readSensor();
return true;
case State::STAGE_5: // Keep reading.
return true;
default:
break;
}
break;
default:
break;
}
return false;
}
return true;
}
