bool Task::perception_state_machine(const base::Time& ts)
{
if(orientation_sample_recieved){ //we have a valid orientation
if(current_depth < _minimum_depth.get()){ //we have a valid depth
if(last_motion.isNull() || ts.toSeconds() - last_motion.toSeconds() > _reset_timeout.get()) //Joint timeout
changeState(NO_JOINTS_NO_DVL);
else if(last_hough.isNull() || ts.toSeconds() - last_hough.toSeconds() > _hough_timeout.get()){ //Hough timeout
changeState(NO_HOUGH);
//get timedifference to last hpugh timeout, and intersper random particles after a given time
if(ts.toSeconds() - last_hough.toSeconds() > _hough_timeout.get()){
if(last_hough_timeout.isNull()){
last_hough_timeout = ts;
}
else if(ts.toSeconds() - last_hough_timeout.toSeconds() > _hough_timeout.get()){
last_hough_timeout = ts;
localizer->interspersal(base::samples::RigidBodyState(), *map, _hough_timeout_interspersal.get(), true, true);
}
}
}
else //Everything is fine :-)
changeState(LOCALIZING);
last_perception = ts;
if(number_rejected_samples < static_cast<unsigned>(_init_sample_rejection.value())) {
number_rejected_samples++;
return false;
}
return true;
}else{ //Invalid depth
changeState(ABOVE_SURFACE);
}
}
else{ //Invalid or no orientation
changeState(NO_ORIENTATION);
}
return false;
}
void calculateSamples(int nr)
{
base::Time sampleLatencyMaxNoise;
if (nr > 0)
sampleLatencyMaxNoise = this->sampleLatencyMaxNoise;
else
sampleLatencyMaxNoise = realPeriod * 0.09;
base::Time sampleLatencyNoise = base::Time::fromSeconds(drand48() * sampleLatencyMaxNoise.toSeconds());
base::Time hwTimeNoise(base::Time::fromSeconds(drand48() * hwTimeMaxNoise.toSeconds()));
actualPeriod = realPeriod + periodDrift * nr;
realTime = baseTime + realPeriod * nr + periodDrift * nr * (nr + 1) / 2;
sampleTime = realTime + sampleLatency + sampleLatencyNoise;
hwTime = realTime + hwTimeNoise;
}
void addResultToPlot(base::Time estimatedTime, base::Time estimatedPeriod)
{
static base::Time lastEstimatedTime = estimatedTime;
static base::Time lastRealTime = realTime;
if(debugFile.is_open())
{
debugFile << (hwTime - realTime).toSeconds() / actualPeriod.toSeconds()
<< " " << (sampleTime - realTime - sampleLatency).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedTime - realTime).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedPeriod - actualPeriod).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedTime - lastEstimatedTime).toSeconds()
<< " " << (realTime - lastRealTime).toSeconds()
<< " " << actualPeriod.toSeconds()
<< std::endl;
}
lastEstimatedTime = estimatedTime;
lastRealTime = realTime;
}
void FDStream::waitWrite(base::Time const& timeout)
{
fd_set set;
FD_ZERO(&set);
FD_SET(m_fd, &set);
timeval timeout_spec = { static_cast<time_t>(timeout.toSeconds()), timeout.toMicroseconds() % 1000000 };
int ret = select(m_fd + 1, NULL, &set, NULL, &timeout_spec);
if (ret < 0 && errno != EINTR)
throw UnixError("waitWrite(): error in select()");
else if (ret == 0)
throw TimeoutError(TimeoutError::NONE, "waitWrite(): timeout");
}
inline void step_dt(const base::Time& dt){
if(mode == base::JointState::POSITION)
{
j_state.position = j_setpoint.position;
j_state.speed = j_setpoint.speed;
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::POSITION);
trunc_to_limit(base::JointState::SPEED);
trunc_to_limit(base::JointState::EFFORT);
}
else if(mode == base::JointState::SPEED)
{
j_state.speed = j_setpoint.speed;
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::SPEED);
trunc_to_limit(base::JointState::EFFORT);
j_state.position += j_setpoint.speed*dt.toSeconds();
trunc_to_limit(base::JointState::POSITION);
}
else if(mode == base::JointState::EFFORT)
{
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::EFFORT);
j_state.speed += j_setpoint.effort*dt.toSeconds();
j_state.position += j_setpoint.speed*dt.toSeconds();
trunc_to_limit(base::JointState::POSITION);
trunc_to_limit(base::JointState::SPEED);
}
perturb(dt.toSeconds());
}
void Task::thruster_samplesCallback(const base::Time& ts, const base::samples::Joints& status)
{
//base::Time temp = base::Time::now();
if(last_speed_time.isNull() || ts.toSeconds() - last_speed_time.toSeconds() > _speed_samples_timeout.get() ){
base::samples::Joints j = status;
last_motion = ts;
//If we have no 6 thruster, fill up the joints with zeros
/*if(j.size() < 6){
unsigned int size = j.size();
j.elements.resize(6);
j.names.resize(6);
for(; size < 6; size++){
j.elements[size].raw = 0.0;
}
}*/
if(status.hasNames()){
for(unsigned int i = 0; i < status.size() && i < config.joint_names.size(); i++){
try{
j.elements[i] = status[config.joint_names[i]];
}
catch(...){
}
}
}
if(orientation_sample_recieved){
localizer->update_dead_reckoning(j);
localizer->update(j, *map);
}else{
changeState(NO_ORIENTATION);
}
}
//std::cout << "Calc time thruster samples: " << base::Time::now().toSeconds() - temp.toSeconds() << std::endl;
}
void checkResult(base::Time estimatedTime, base::Time estimatedPeriod)
{
addResultToPlot(estimatedTime, estimatedPeriod);
BOOST_CHECK_SMALL((estimatedTime - realTime).toSeconds(), actualPeriod.toSeconds() / 10);
}