/**
* 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 Lateral::compute(const base::commands::Motion2D &motionCmd, base::samples::Joints& actuatorsCommand)
{
double speed = translateSpeedToWheelSpeed(motionCmd.translation);
for (auto jointActuator: controllerBase->getJointActuators())
{
JointCmd* positionCmd = jointActuator->getJointCmdForType(JointCmdType::Position);
JointCmd* steeringCmd = jointActuator->getJointCmdForType(JointCmdType::Speed);
if (!positionCmd || !steeringCmd)
{
continue;
}
base::JointState &positionJointState(actuatorsCommand[actuatorsCommand.mapNameToIndex(positionCmd->getName())]);
base::JointState &steeringJointState(actuatorsCommand[actuatorsCommand.mapNameToIndex(steeringCmd->getName())]);
positionJointState.position = motionCmd.heading.getRad();
steeringJointState.speed = speed;
if(motionCmd.heading.getRad() > M_PI){
positionJointState.position -= M_PI;
positionJointState.speed *= -1;
}
}
return true;
}
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;
}
}
}
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);
}
}
}
