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 


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Dec 2 2013 13:13:02