/** * 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 TransformationCalculator::update(const base::samples::Joints& joints) { base::JointState j_state; std::string j_name; std::string seg_name; KDL::Segment seg; KDL::Frame kdl_pose; std::map<std::string,base::samples::RigidBodyState>::iterator transform_it; if(!is_initialized_) throw std::runtime_error("TransformationCalculator was not initialized. Call load_robot_model first"); for(uint i=0; i<joints.size(); i++){ j_state = joints.elements[i]; j_name = joints.names[i]; if(is_invalid(j_state.position)){ LOG_ERROR("Received invalid joint position for joint %s", j_name.c_str()); continue; } try{ //Get segment seg_name = joint_name2seg_name_[j_name]; seg = get_segment_by_segment_name(seg_name); //Calculate pose kdl_pose = seg.pose(j_state.position); //Convert transform to eigen transform_it = moving_joints_transforms_.find(j_name); if(transform_it == moving_joints_transforms_.end()) throw("Unknown frame name. Should be thrown at this point, but ealier in get_segment."); convert(kdl_pose, transform_it->second); //Set timestap transform_it->second.time = joints.time; } catch(...){ LOG_WARN("Joint %s or segment %s are unknown", j_name.c_str(), seg_name.c_str()); continue; } } }