$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 <kdl/tree.hpp> 00038 #include <ros/ros.h> 00039 #include "robot_state_publisher/robot_state_publisher.h" 00040 #include "robot_state_publisher/joint_state_listener.h" 00041 #include <kdl_parser/kdl_parser.hpp> 00042 00043 00044 using namespace std; 00045 using namespace ros; 00046 using namespace KDL; 00047 using namespace robot_state_publisher; 00048 00049 00050 JointStateListener::JointStateListener(const KDL::Tree& tree) 00051 : state_publisher_(tree) 00052 { 00053 ros::NodeHandle n_tilde("~"); 00054 ros::NodeHandle n; 00055 00056 // set publish frequency 00057 double publish_freq; 00058 n_tilde.param("publish_frequency", publish_freq, 50.0); 00059 publish_interval_ = ros::Duration(1.0/max(publish_freq,1.0)); 00060 00061 00062 // subscribe to joint state 00063 joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this); 00064 00065 // trigger to publish fixed joints 00066 timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this); 00067 00068 }; 00069 00070 00071 JointStateListener::~JointStateListener() 00072 {}; 00073 00074 00075 void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e) 00076 { 00077 state_publisher_.publishFixedTransforms(); 00078 } 00079 00080 void JointStateListener::callbackJointState(const JointStateConstPtr& state) 00081 { 00082 if (state->name.size() != state->position.size()){ 00083 ROS_ERROR("Robot state publisher received an invalid joint state vector"); 00084 return; 00085 } 00086 00087 // determine least recently published joint 00088 ros::Time last_published = ros::Time::now(); 00089 for (unsigned int i=0; i<state->name.size(); i++) 00090 { 00091 ros::Time t = last_publish_time_[state->name[i]]; 00092 last_published = (t < last_published) ? t : last_published; 00093 } 00094 // note: if a joint was seen for the first time, 00095 // then last_published is zero. 00096 00097 00098 // check if we need to publish 00099 if (state->header.stamp >= last_published + publish_interval_){ 00100 // get joint positions from state message 00101 map<string, double> joint_positions; 00102 for (unsigned int i=0; i<state->name.size(); i++) 00103 joint_positions.insert(make_pair(state->name[i], state->position[i])); 00104 state_publisher_.publishTransforms(joint_positions, state->header.stamp); 00105 00106 // store publish time in joint map 00107 for (unsigned int i=0; i<state->name.size(); i++) 00108 last_publish_time_[state->name[i]] = state->header.stamp; 00109 } 00110 } 00111 00112 00113 00114 00115 00116 // ---------------------------------- 00117 // ----- MAIN ----------------------- 00118 // ---------------------------------- 00119 int main(int argc, char** argv) 00120 { 00121 // Initialize ros 00122 ros::init(argc, argv, "robot_state_publisher"); 00123 NodeHandle node; 00124 00125 // gets the location of the robot description on the parameter server 00126 KDL::Tree tree; 00127 if (!kdl_parser::treeFromParam("robot_description", tree)){ 00128 ROS_ERROR("Failed to extract kdl tree from xml robot description"); 00129 return -1; 00130 } 00131 00132 JointStateListener state_publisher(tree); 00133 ros::spin(); 00134 00135 return 0; 00136 }