47 KDL::SegmentMap segmentMap;
52 ROS_ERROR(
"Failed to construct KDL:Tree from robot_state's URDF model! Aborting ...");
56 ROS_INFO(
"KDL::Tree successful created.");
65 std::map<unsigned int, std::string> jointMap;
67 ROS_DEBUG(
"Extracting all joints from the tree, which are not of type KDL::Joint::None.");
68 for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
70 if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
71 jointMap[seg_it->second.q_nr] = seg_it->second.segment.getJoint().getName().c_str();
75 ROS_DEBUG(
"Checking, if extracted joints can be found in the JointState vector of the robot.");
77 for (std::map<unsigned int, std::string>::const_iterator jnt_it = jointMap.begin();
78 jnt_it != jointMap.end(); ++jnt_it)
83 ROS_ERROR(
"Joint '%s' has not been found in the robot's joint state vector! Aborting ...",
84 jnt_it->second.c_str());
90 ROS_DEBUG(
"The result after joint extraction and checking:");
91 for (
unsigned int i = 0; i <
joints_.size(); ++i)
93 ROS_DEBUG(
"joints_[%d]: joint_.name = %s", i,
joints_[i]->joint_->name.c_str());