treefksolverposfull_recursive.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2009 Willow Garage Inc
00002 
00003 // Version: 1.0
00004 // Author: Wim Meeussen <meeussen at willowgarage dot com>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 
00023 #include "robot_state_publisher/treefksolverposfull_recursive.hpp"
00024 #include <ros/ros.h>
00025 #include <iostream>
00026 #include <cstdio>
00027 
00028 using namespace std;
00029 
00030 namespace KDL {
00031 
00032 TreeFkSolverPosFull_recursive::TreeFkSolverPosFull_recursive(const Tree& _tree):
00033   tree(_tree)
00034 {
00035 }
00036 
00037 TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
00038 {
00039 }
00040 
00041 
00042 int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
00043 {      
00044   // clear output
00045   p_out.clear();
00046 
00047   addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), tree.getRootSegment()->second.segment.getName()), tree.getRootSegment(), flatten_tree);
00048 
00049   return 0;
00050 }
00051 
00052 
00053                                              
00054 void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in, 
00055                                                   map<string, tf::Stamped<Frame> >& p_out,
00056                                                   const tf::Stamped<KDL::Frame>& previous_frame,
00057                                                   const SegmentMap::const_iterator this_segment,
00058                                                   bool flatten_tree)
00059 {
00060   // get pose of this segment
00061   tf::Stamped<KDL::Frame> this_frame;
00062   double jnt_p = 0;
00063   if (this_segment->second.segment.getJoint().getType() != Joint::None){
00064     map<string, double>::const_iterator jnt_pos = q_in.find(this_segment->second.segment.getJoint().getName());
00065     if (jnt_pos == q_in.end()){
00066       ROS_DEBUG("Warning: TreeFKSolverPosFull Could not find value for joint '%s'. Skipping this tree branch", this_segment->first.c_str());
00067       return;
00068     }
00069     jnt_p = jnt_pos->second;
00070   }
00071   this_frame = tf::Stamped<KDL::Frame>(previous_frame * this_segment->second.segment.pose(jnt_p), ros::Time(), previous_frame.frame_id_);
00072 
00073   if (this_segment->first != tree.getRootSegment()->first)
00074     p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
00075 
00076   // get poses of child segments
00077   for (vector<SegmentMap::const_iterator>::const_iterator child=this_segment->second.children.begin(); child !=this_segment->second.children.end(); child++){
00078     if (flatten_tree)
00079       addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
00080     else
00081       addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
00082   }      
00083 }
00084 
00085 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


robot_state_publisher
Author(s): Wim Meeussen meeussen@willowgarage.com
autogenerated on Mon Aug 19 2013 11:02:17