bool framesFromKDLTree(const KDL::Tree& tree,
std::vector<std::string>& framesNames,
std::vector<std::string>& parentLinkNames)
{
framesNames.clear();
parentLinkNames.clear();
KDL::SegmentMap::iterator seg;
KDL::SegmentMap segs;
KDL::SegmentMap::const_iterator root_seg;
root_seg = tree.getRootSegment();
segs = tree.getSegments();
for( seg = segs.begin(); seg != segs.end(); seg++ )
{
if( GetTreeElementChildren(seg->second).size() == 0 &&
GetTreeElementSegment(seg->second).getJoint().getType() == KDL::Joint::None &&
GetTreeElementSegment(seg->second).getInertia().getMass() == 0.0 )
{
std::string frameName = GetTreeElementSegment(seg->second).getName();
std::string parentLinkName = GetTreeElementSegment(GetTreeElementParent(seg->second)->second).getName();
framesNames.push_back(frameName);
parentLinkNames.push_back(parentLinkName);
//also check parent
KDL::Segment parent = GetTreeElementSegment(GetTreeElementParent(seg->second)->second);
if (parent.getJoint().getType() == KDL::Joint::None &&
parent.getInertia().getMass() == 0.0)
{
framesNames.push_back(parentLinkName);
}
}
}
return true;
}
bool isBaseLinkFake(const KDL::Tree & tree)
{
KDL::SegmentMap::const_iterator root = tree.getRootSegment();
bool return_value;
if (GetTreeElementChildren(root->second).size() == 1 && GetTreeElementSegment(GetTreeElementChildren(root->second)[0]->second).getJoint().getType() == Joint::None) {
return_value = true;
} else {
return_value = false;
}
return return_value;
}
void JointStateConverter::addChildren(const KDL::SegmentMap::const_iterator segment)
{
const std::string& root = GetTreeElementSegment(segment->second).getName();
const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
for (unsigned int i=0; i<children.size(); i++){
const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
robot_state_publisher::SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
if (child.getJoint().getType() == KDL::Joint::None){
segments_fixed_.insert(std::make_pair(child.getJoint().getName(), s));
ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
}
else{
segments_.insert(std::make_pair(child.getJoint().getName(), s));
ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
}
addChildren(children[i]);
}
}
void Robot::propagateTree(
const KDL::SegmentMap::const_iterator segment, const KDL::Frame frame,
const std::unordered_map<std::string, double> &joint_state) {
const std::string &root = GetTreeElementSegment(segment->second).getName();
auto joint = GetTreeElementSegment(segment->second).getJoint();
double joint_angle = (joint.getType() == KDL::Joint::None)
? 0.0
: joint_state.at(joint.getName());
KDL::Frame child_frame = frame * (segment->second.segment.pose(joint_angle));
link_frames_[root] = child_frame;
const std::vector<KDL::SegmentMap::const_iterator> &children =
GetTreeElementChildren(segment->second);
for (unsigned int i = 0; i < children.size(); i++) {
propagateTree(children[i], child_frame, joint_state);
}
}
