Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <urdf/model.h>
00038 #include <kdl/tree.hpp>
00039 #include <ros/ros.h>
00040 #include "robot_state_publisher/robot_state_publisher.h"
00041 #include "robot_state_publisher/joint_state_listener.h"
00042 #include <kdl_parser/kdl_parser.hpp>
00043
00044
00045 using namespace std;
00046 using namespace ros;
00047 using namespace KDL;
00048 using namespace robot_state_publisher;
00049
00050 JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m)
00051 : state_publisher_(tree), mimic_(m)
00052 {
00053 ros::NodeHandle n_tilde("~");
00054 ros::NodeHandle n;
00055
00056
00057 double publish_freq;
00058 n_tilde.param("publish_frequency", publish_freq, 50.0);
00059
00060 std::string tf_prefix_key;
00061 n_tilde.searchParam("tf_prefix", tf_prefix_key);
00062 n_tilde.param(tf_prefix_key, tf_prefix_, std::string(""));
00063 publish_interval_ = ros::Duration(1.0/max(publish_freq,1.0));
00064
00065
00066 joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
00067
00068
00069 timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
00070
00071 };
00072
00073
00074 JointStateListener::~JointStateListener()
00075 {};
00076
00077
00078 void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
00079 {
00080 state_publisher_.publishFixedTransforms(tf_prefix_);
00081 }
00082
00083 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
00084 {
00085 if (state->name.size() != state->position.size()){
00086 ROS_ERROR("Robot state publisher received an invalid joint state vector");
00087 return;
00088 }
00089
00090
00091 ros::Time last_published = ros::Time::now();
00092 for (unsigned int i=0; i<state->name.size(); i++)
00093 {
00094 ros::Time t = last_publish_time_[state->name[i]];
00095 last_published = (t < last_published) ? t : last_published;
00096 }
00097
00098
00099
00100
00101
00102 if (state->header.stamp >= last_published + publish_interval_){
00103
00104 map<string, double> joint_positions;
00105 for (unsigned int i=0; i<state->name.size(); i++)
00106 joint_positions.insert(make_pair(state->name[i], state->position[i]));
00107
00108 for(MimicMap::iterator i = mimic_.begin(); i != mimic_.end(); i++){
00109 if(joint_positions.find(i->second->joint_name) != joint_positions.end()){
00110 double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
00111 joint_positions.insert(make_pair(i->first, pos));
00112 }
00113 }
00114
00115 state_publisher_.publishTransforms(joint_positions, state->header.stamp, tf_prefix_);
00116
00117
00118 for (unsigned int i=0; i<state->name.size(); i++)
00119 last_publish_time_[state->name[i]] = state->header.stamp;
00120 }
00121 }
00122
00123
00124
00125
00126 int main(int argc, char** argv)
00127 {
00128
00129 ros::init(argc, argv, "robot_state_publisher");
00130 NodeHandle node;
00131 std::cout <<argv[0] << std::endl;
00132
00134 std::string exe_name = argv[0];
00135 std::size_t slash = exe_name.find_last_of("/");
00136 if (slash != std::string::npos)
00137 exe_name = exe_name.substr(slash + 1);
00138 if (exe_name == "state_publisher")
00139 ROS_WARN("The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead");
00141
00142
00143 urdf::Model model;
00144 model.initParam("robot_description");
00145 KDL::Tree tree;
00146 if (!kdl_parser::treeFromUrdfModel(model, tree)){
00147 ROS_ERROR("Failed to extract kdl tree from xml robot description");
00148 return -1;
00149 }
00150
00151 MimicMap mimic;
00152
00153 for(std::map< std::string, boost::shared_ptr< urdf::Joint > >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++){
00154 if(i->second->mimic){
00155 mimic.insert(make_pair(i->first, i->second->mimic));
00156 }
00157 }
00158
00159 JointStateListener state_publisher(tree, mimic);
00160 ros::spin();
00161
00162 return 0;
00163 }