Beispiel #1
0
 /**
  * Makes sure that all jointstate are within their respective limits
  *
  * Will throw if this is not the case. Will also throw if there are no
  * limits for a particular joint.
  */
 void validate( const base::samples::Joints& joints ) const
 {
     if (joints.hasNames())
     {
         for( size_t i=0; i<joints.size(); i++ )
             (*this)[joints.names[i]].validate( joints[i] );
     }
     else
     {
         for( size_t i=0; i<joints.size(); i++ )
             (*this)[i].validate( joints[i] );
     }
 }
Beispiel #2
0
 bool isValid( const base::samples::Joints& joints ) const
 {
     if (joints.hasNames())
     {
         for( size_t i=0; i<joints.size(); i++ )
             if (! (*this)[joints.names[i]].isValid( joints[i] ))
                 return false;
     }
     else
     {
         for( size_t i=0; i<joints.size(); i++ )
             if (! (*this)[i].isValid( joints[i] ))
                 return false;
     }
     return true;
 }
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 RobotVisualization::setJointsState(const base::samples::Joints &sample){
    vector<string> names = getJointNames();
    if (sample.hasNames())
        names = sample.names;
    if (names.size() != sample.elements.size())
        throw std::runtime_error("RobotVisualization::updateDataIntern: state vector size and expected joint size differ, and there are no names in the Joints sample");

    for(uint i=0; i<names.size(); i++){
        if(base::isUnknown(sample[i].position) || base::isInfinity(sample[i].position)){
            if(names.size()){
                LOG_ERROR("Position of joint %s is invalid: %d", sample.names[i].c_str(), sample[i].position);
            }
            else{
                LOG_ERROR("Position of joint %d is invalid: %d", i, sample[i].position);
            }
            //throw std::runtime_error("RobotVisualization::updateDataIntern: invalid joint position detected.");
            setJointState(names[i], 0);
        }
        else{
            setJointState(names[i], sample[i].position);
        }
    }
}