/**
* 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] );
}
}
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);
}
}
}
