Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <kdl_parser/kdl_parser.hpp>
00040 #include "pr2_mechanism_model/tree.h"
00041
00042 namespace pr2_mechanism_model
00043 {
00044
00045 bool Tree::init(RobotState *robot_state)
00046 {
00047 KDL::SegmentMap segmentMap;
00048
00049
00050 if (!kdl_parser::treeFromUrdfModel(robot_state->model_->robot_model_, kdl_tree_))
00051 {
00052 ROS_ERROR("Failed to construct KDL:Tree from robot_state's URDF model! Aborting ...");
00053 }
00054 else
00055 {
00056 ROS_INFO("KDL::Tree successful created.");
00057
00058 segmentMap = kdl_tree_.getSegments();
00059 }
00060
00061
00062
00063
00064
00065 std::map<unsigned int, std::string> jointMap;
00066
00067 ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
00068 for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
00069 {
00070 if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
00071 jointMap[seg_it->second.q_nr] = seg_it->second.segment.getJoint().getName().c_str();
00072 }
00073
00074
00075 ROS_DEBUG("Checking, if extracted joints can be found in the JointState vector of the robot.");
00076 joints_.clear();
00077 for (std::map<unsigned int, std::string>::const_iterator jnt_it = jointMap.begin();
00078 jnt_it != jointMap.end(); ++jnt_it)
00079 {
00080 JointState* jnt = robot_state->getJointState(jnt_it->second.c_str());
00081 if (!jnt)
00082 {
00083 ROS_ERROR("Joint '%s' has not been found in the robot's joint state vector! Aborting ...",
00084 jnt_it->second.c_str());
00085 return false;
00086 }
00087 joints_.push_back(jnt);
00088 }
00089
00090 ROS_DEBUG("The result after joint extraction and checking:");
00091 for (unsigned int i = 0; i < joints_.size(); ++i)
00092 {
00093 ROS_DEBUG("joints_[%d]: joint_.name = %s", i, joints_[i]->joint_->name.c_str());
00094 }
00095
00096 ROS_INFO("Added %i joints", int(joints_.size()));
00097
00098 return true;
00099 }
00100
00101 }
00102