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;
    }
Ejemplo n.º 4
0
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);
 }