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 <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
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
00063 joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
00064
00065
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
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
00095
00096
00097
00098
00099 if (state->header.stamp >= last_published + publish_interval_){
00100
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
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
00118
00119 int main(int argc, char** argv)
00120 {
00121
00122 ros::init(argc, argv, "robot_state_publisher");
00123 NodeHandle node;
00124 std::cout <<argv[0] << std::endl;
00125
00127 std::string exe_name = argv[0];
00128 std::size_t slash = exe_name.find_last_of("/");
00129 if (slash != std::string::npos)
00130 exe_name = exe_name.substr(slash + 1);
00131 if (exe_name == "state_publisher")
00132 ROS_WARN("The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead");
00134
00135
00136 KDL::Tree tree;
00137 if (!kdl_parser::treeFromParam("robot_description", tree)){
00138 ROS_ERROR("Failed to extract kdl tree from xml robot description");
00139 return -1;
00140 }
00141
00142 JointStateListener state_publisher(tree);
00143 ros::spin();
00144
00145 return 0;
00146 }