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");
}
Example #2
0
    bool TouchPlatformFile(PlatformFile file, const base::Time& last_access_time,
        const base::Time& last_modified_time)
    {
        ThreadRestrictions::AssertIOAllowed();
        if(file == kInvalidPlatformFileValue)
        {
            return false;
        }

        FILETIME last_access_filetime = last_access_time.ToFileTime();
        FILETIME last_modified_filetime = last_modified_time.ToFileTime();
        return (SetFileTime(file, NULL, &last_access_filetime,
            &last_modified_filetime) != 0);
    }
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 PortStream::waitRead(base::Time const& timeout)
{
    if (!mPacketRead.data.empty())
        return;

    uint64_t sleep_time;
    int count;
    uint64_t full_timeout = timeout.toMicroseconds();
    if (full_timeout > 10000)
    {
        sleep_time = 10000;
        count = (full_timeout + sleep_time - 1) / sleep_time;
    }
    else
    {
        count = 1;
        sleep_time = full_timeout;
    }

    for (int i = 0; i < count; ++i)
    {
        if (mIn.read(mPacketRead, false) == RTT::NewData)
            return;
        usleep(sleep_time);
    }
    throw TimeoutError(TimeoutError::NONE, "waitRead(): timeout");
}
    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 Task::buoy_samplesCallback(const base::Time& ts, const avalon::feature::Buoy& buoy){
  
   if(!_use_slam.get() ){
          
      if(lastRBS.cov_position(0,0) <= _position_covariance_threshold.get() && lastRBS.cov_position(1,1) <= _position_covariance_threshold.get() ){
        
        if(buoy.color == avalon::feature::NO_BUOY){
          return;
        }
        
        BuoyColor bc;
        
        if(buoy.color == avalon::feature::WHITE){
          bc = WHITE;
        }else if(buoy.color == avalon::feature::ORANGE){
          bc = ORANGE;
        }
        else if(buoy.color == avalon::feature::UNKNOWN){
          bc = UNKNOWN;
        }
        
        base::Vector3d buoyPose = lastRBS.position + (lastRBS.orientation * config.buoyCamPosition);
        
        if(grid_map->setBuoy(buoyPose.x(), buoyPose.y(), bc , buoy.probability, true)){
          std::cout << "BOJE!" << "(" << ts.toString() << "," << buoyPose.x() << "," << buoyPose.y() << "," << buoyPose.z() << ",FOUND_BUOY)" << std::endl;
        }
        
      }
  }  
  
  
  //double effective_sample_size = localizer->observe(buoy, *map, _buoy_importance.value());
  
}
    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;
    }
    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 ros_convertions::toROS( ros::Time& ros, ::base::Time const& value )
{
    ros.fromNSec(value.toMicroseconds() * 1000);
}
 void checkResult(base::Time estimatedTime, base::Time estimatedPeriod)
 {
     addResultToPlot(estimatedTime, estimatedPeriod);
     BOOST_CHECK_SMALL((estimatedTime - realTime).toSeconds(), actualPeriod.toSeconds() / 10);
 }
void SoftTurnController::updateHook()
{
    SoftTurnControllerBase::updateHook();
    
    double pointTurnLimit = 0.1;
    double stepLimit = 0.01;

    static double lastSpeed = 0;
    static base::Time changeTime;

    base::actuators::Status status;
    if(_status.readNewest(status, true) == RTT::NoData)
	return;

    // This is the user's command
    base::commands::Motion2D cmd_in;
    if (_motion_command.readNewest(cmd_in, true) == RTT::NoData)
    {
        cmd_in.translation = 0;
        cmd_in.rotation    = 0;
    }
        
    //let's allways turn
    if(true || fabs(cmd_in.rotation) > pointTurnLimit)
    {
        //hack
        cmd_in.rotation = 1.0;
        cmd_in.rotation = turnSpeed * fabs(cmd_in.rotation) / cmd_in.rotation;

	static bool switchState = false;
	for(int i = 0; i < 4; i++)
	{
	    const double diff = fabs(startStatus.states[i].positionExtern - status.states[i].positionExtern);
	    //we assume zero slip
	    if(diff * _wheel_radius.get()  > _turnVariance.get() )
	    {
		//we moved the maximum variance in one direction, reverse
		switchState = true;
		break;
	    }
	}
	
	if(!switchState)
	{
        switch(state)
        {
            case FORWARD:
                cmd_in.translation = translationalSpeed;		
                break;
                
            case BACKWARD:
                cmd_in.translation = -translationalSpeed;		
                break;
        }
	}
	else 
	{
	    if(changeTime.isNull())
		changeTime = base::Time::now();

	    if(base::Time::now() - changeTime > base::Time::fromSeconds(0.5) )
	    {
		startStatus = status;
		if(state == FORWARD)
		    state = BACKWARD;
		else
		    state = FORWARD;

		std::cout << "Switched State" << std::endl;
		changeTime = base::Time();
		switchState = false;
	    }
	}
    }

    double newSpeed = 0;
    if(cmd_in.translation < 0)
    {
	newSpeed = lastSpeed - stepLimit;
	if(newSpeed < cmd_in.translation)
	    newSpeed = cmd_in.translation;
    }
    else
    {
	newSpeed = lastSpeed + stepLimit;
	if(newSpeed > cmd_in.translation)
	    newSpeed = cmd_in.translation;
    }

    lastSpeed = newSpeed;
    cmd_in.translation = newSpeed;

    std::cout << "cmd_in.translation " << cmd_in.translation << "cmd_in.rotation " << cmd_in.rotation << std::endl; 
    double fwd_velocity = cmd_in.translation / _wheel_radius.get();
    double differential = cmd_in.rotation * _track_width.get() / _wheel_radius.get();
    
    double leftSpeed;
    double rightSpeed;
    
    m_cmd.mode[0] = m_cmd.mode[1] =
        m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;

    
    m_cmd.target[base::actuators::WHEEL4_FRONT_LEFT]  = fwd_velocity - differential;
    m_cmd.target[base::actuators::WHEEL4_REAR_LEFT]   = fwd_velocity - differential;
    m_cmd.target[base::actuators::WHEEL4_FRONT_RIGHT] = fwd_velocity + differential;
    m_cmd.target[base::actuators::WHEEL4_REAR_RIGHT]  = fwd_velocity + differential;
    m_cmd.time = base::Time::now();

    _simple_command.write(m_cmd);
}