$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Wim Meeussen */ 00036 00037 #include "robot_state_chain_publisher/robot_state_publisher.h" 00038 #include <kdl/frames_io.hpp> 00039 #include <tf_conversions/tf_kdl.h> 00040 00041 using namespace std; 00042 using namespace ros; 00043 using namespace KDL; 00044 00045 00046 00047 namespace robot_state_chain_publisher{ 00048 00049 RobotStatePublisher::RobotStatePublisher(const Tree& tree) 00050 :n_("~"), tree_(tree) 00051 { 00052 // get tf prefix 00053 n_.param("tf_prefix", tf_prefix_, string()); 00054 00055 // advertise tf message 00056 tf_publisher_ = n_.advertise<tf::tfMessage>("/tf", 5); 00057 tf_msg_.transforms.resize(tree.getNrOfSegments()-1); 00058 00059 // get the 'real' root segment of the tree, which is the first child of "root" 00060 SegmentMap::const_iterator root = tree.getRootSegment(); 00061 if (root->second.children.empty()) 00062 throw empty_tree_ex; 00063 00064 root_ = root->first; 00065 ignore_root = false; 00066 00067 if (root_ == "world") 00068 { 00069 ignore_root = true; 00070 root_ = (*root->second.children.begin())->first; 00071 } 00072 } 00073 00074 00075 00076 bool RobotStatePublisher::publishTransforms(map<string, RobotStatePublisher::JointState >& joint_positions, const Time& time, const ros::Time& republish_time) 00077 { 00078 int i=0; 00079 if (ignore_root) 00080 tf_msg_.transforms.resize(tree_.getNrOfSegments()-1); 00081 else 00082 tf_msg_.transforms.resize(tree_.getNrOfSegments()); 00083 addChildTransforms( joint_positions, tree_.getSegment( root_ ), i, time, republish_time ); 00084 tf_msg_.transforms.resize(i); 00085 00086 tf_publisher_.publish(tf_msg_); 00087 00088 return true; 00089 } 00090 00091 void RobotStatePublisher::addChildTransforms(map<string, RobotStatePublisher::JointState >& joint_positions, const SegmentMap::const_iterator segment, int &tf_index, const Time &time, const ros::Time& republish_time) 00092 { 00093 double jnt_p = 0; 00094 bool publish=true; 00095 geometry_msgs::TransformStamped trans; 00096 00097 for( vector<SegmentMap::const_iterator>::const_iterator child=segment->second.children.begin(); 00098 child != segment->second.children.end(); child++ ) 00099 { 00100 Time time_frame = time; 00101 if( (*child)->second.segment.getJoint().getType() != Joint::None ) 00102 { 00103 map<string, RobotStatePublisher::JointState >::iterator jnt = joint_positions.find((*child)->second.segment.getJoint().getName()); 00104 if (jnt == joint_positions.end()){ 00105 jnt_p = 0; 00106 } 00107 else { 00108 jnt_p = jnt->second.pos; 00109 00110 // if it's time for republishing, update timestamp 00111 if( jnt->second.time < republish_time ) { 00112 jnt->second.time = time; 00113 jnt->second.published = false; 00114 } 00115 00116 time_frame = jnt->second.time; 00117 publish = ! jnt->second.published; 00118 jnt->second.published = true; 00119 } 00120 } 00121 00122 // only publish new transforms 00123 if( publish ) { 00124 Frame frame = (*child)->second.segment.pose( jnt_p ); 00125 tf::Transform tf_frame; 00126 tf::TransformKDLToTF(frame, tf_frame); 00127 trans.header.stamp = time_frame; 00128 trans.header.frame_id = tf::resolve(tf_prefix_, segment->first); 00129 trans.child_frame_id = tf::resolve(tf_prefix_, (*child)->first); 00130 if (trans.child_frame_id == string("")) 00131 { 00132 00133 } 00134 tf::transformTFToMsg(tf_frame, trans.transform); 00135 tf_msg_.transforms[tf_index++] = trans; 00136 } 00137 00138 addChildTransforms(joint_positions, *child, tf_index, time, republish_time); 00139 } 00140 } 00141 00142 }