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); }