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_chain_publisher/robot_state_publisher.h"
00040 #include "robot_state_chain_publisher/joint_state_listener.h"
00041
00042 using namespace std;
00043 using namespace ros;
00044 using namespace KDL;
00045 using namespace robot_state_chain_publisher;
00046
00047
00048 JointStateListener::JointStateListener(const KDL::Tree& tree)
00049 : n_tilde_("~"), state_publisher_(tree)
00050 {
00051
00052 double publish_freq, min_publish_freq;
00053 n_tilde_.param("publish_frequency", publish_freq, 50.0);
00054 n_tilde_.param("min_publish_frequency", min_publish_freq, 1.0);
00055 publish_delay_ = Duration(1/publish_freq);
00056 max_publish_delay_ = Duration(1/min_publish_freq);
00057
00058 last_publish_time_ = Time::now();
00059
00060 joint_state_sub_ = n_.subscribe("joint_states", 10, &JointStateListener::callbackJointState, this);
00061
00062 timer_ = n_tilde_.createTimer(max_publish_delay_, &JointStateListener::timerCallback, this);
00063 };
00064
00065
00066 JointStateListener::~JointStateListener()
00067 {};
00068
00069
00070 void JointStateListener::timerCallback(const ros::TimerEvent& e)
00071 {
00072 Time now = Time::now();
00073 if( now >= last_publish_time_ + max_publish_delay_*0.9 )
00074 {
00075
00076 state_publisher_.publishTransforms(joint_positions, now, (now - max_publish_delay_));
00077
00078 if( last_publish_time_ - now > max_publish_delay_*2 || last_publish_time_ - now < max_publish_delay_*2)
00079 last_publish_time_ = now;
00080 else
00081 last_publish_time_ += max_publish_delay_;
00082 }
00083 }
00084
00085 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
00086 {
00087 if (state->name.size() == 0){
00088 ROS_ERROR("Robot state publisher received an empty joint state vector");
00089 return;
00090 }
00091
00092 if (state->name.size() != state->position.size()){
00093 ROS_ERROR("Robot state publisher received an invalid joint state vector");
00094 return;
00095 }
00096
00097
00098 if(last_publish_time_.toSec() <= max_publish_delay_.toSec()) {
00099 last_publish_time_ = Time::now();
00100 return;
00101 }
00102
00103
00104 for (unsigned int i=0; i<state->name.size(); i++) {
00105 RobotStatePublisher::JointState s;
00106 s.pos = state->position[i];
00107 s.time = state->header.stamp;
00108 s.published = false;
00109 joint_positions[state->name[i]] = s;
00110
00111 }
00112
00113 Time now = Time::now();
00114 if( now >= last_publish_time_ + publish_delay_ )
00115 {
00116 state_publisher_.publishTransforms(joint_positions, now, (now - max_publish_delay_));
00117
00118 if( last_publish_time_ - now > publish_delay_*2 || last_publish_time_ - now < publish_delay_*2)
00119 last_publish_time_ = now;
00120 else
00121 last_publish_time_ += publish_delay_;
00122 }
00123 }