tree.cpp
Go to the documentation of this file.00001 /* 00002 * Copyright (c) 2011, PAL Robotics S.L. 00003 * All rights reserved. 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Author: Marcus Liebhardt 00009 * 00010 * This class has been derived from the chain class in the 00011 * pr2_mechanism_model package in the pr2_mechanism stack for ROS 00012 * written by Stuart Glaser and Wim Meeussen. 00013 * 00014 * Redistribution and use in source and binary forms, with or without 00015 * modification, are permitted provided that the following conditions are met: 00016 * 00017 * * Redistributions of source code must retain the above copyright 00018 * notice, this list of conditions and the following disclaimer. 00019 * * Redistributions in binary form must reproduce the above copyright 00020 * notice, this list of conditions and the following disclaimer in the 00021 * documentation and/or other materials provided with the distribution. 00022 * * Neither the name of the Willow Garage, Inc. nor the names of its 00023 * contributors may be used to endorse or promote products derived from 00024 * this software without specific prior written permission. 00025 * 00026 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00027 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00028 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00029 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00030 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00031 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00032 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00033 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00034 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00035 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00036 * POSSIBILITY OF SUCH DAMAGE. 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 // construct the kdl tree 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 // the first step of extracting the joints from the tree is to go through all tree_elements, check for a joint, 00062 // and check in case a joint is found, if it is not of not of type KDL::Joint::None 00063 00064 // map for saving the temporary result of the joint extraction from the tree 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 // in the second step the joints found get checked, if they appear in the JointState vector of the robot 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 } // namespace 00102