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)
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());
bool init(RobotState *robot_state)
initializes the tree object The initializer's most important functionality is to create a vector of j...
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
std::map< std::string, TreeElement > SegmentMap
std::vector< JointState * > joints_
a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints) ...
This class provides the controllers with an interface to the robot state.
Robot * model_
The robot model containing the transmissions, urdf robot model, and hardware interface.
JointState * getJointState(const std::string &name)
Get a joint state by name.
const SegmentMap & getSegments() const
urdf::Model robot_model_
The kinematic/dynamic model of the robot.