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 <ros/ros.h>
00038 #include <urdf/model.h>
00039 #include <kdl/tree.hpp>
00040 #include <kdl_parser/kdl_parser.hpp>
00041
00042 #include "robot_state_publisher/robot_state_publisher.h"
00043 #include "robot_state_publisher/joint_state_listener.h"
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, const urdf::Model& model)
00051 : state_publisher_(tree, model), 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 n_tilde.param("use_tf_static", use_tf_static_, true);
00061
00062 std::string tf_prefix_key;
00063 n_tilde.searchParam("tf_prefix", tf_prefix_key);
00064 n_tilde.param(tf_prefix_key, tf_prefix_, std::string(""));
00065 publish_interval_ = ros::Duration(1.0/max(publish_freq, 1.0));
00066
00067
00068 joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
00069
00070
00071
00072 timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this, use_tf_static_);
00073 }
00074
00075
00076 JointStateListener::~JointStateListener()
00077 {}
00078
00079
00080 void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
00081 {
00082 state_publisher_.publishFixedTransforms(tf_prefix_, use_tf_static_);
00083 }
00084
00085 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
00086 {
00087 if (state->name.size() != state->position.size()){
00088 ROS_ERROR("Robot state publisher received an invalid joint state vector");
00089 return;
00090 }
00091
00092
00093 ros::Time now = ros::Time::now();
00094 if (last_callback_time_ > now) {
00095
00096 ROS_WARN("Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!");
00097 last_publish_time_.clear();
00098 }
00099 last_callback_time_ = now;
00100
00101
00102 ros::Time last_published = now;
00103 for (unsigned int i=0; i<state->name.size(); i++) {
00104 ros::Time t = last_publish_time_[state->name[i]];
00105 last_published = (t < last_published) ? t : last_published;
00106 }
00107
00108
00109
00110
00111 if (state->header.stamp >= last_published + publish_interval_) {
00112
00113 map<string, double> joint_positions;
00114 for (unsigned int i=0; i<state->name.size(); i++) {
00115 joint_positions.insert(make_pair(state->name[i], state->position[i]));
00116 }
00117
00118 for (MimicMap::iterator i = mimic_.begin(); i != mimic_.end(); i++) {
00119 if(joint_positions.find(i->second->joint_name) != joint_positions.end()) {
00120 double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
00121 joint_positions.insert(make_pair(i->first, pos));
00122 }
00123 }
00124
00125 state_publisher_.publishTransforms(joint_positions, state->header.stamp, tf_prefix_);
00126
00127
00128 for (unsigned int i = 0; i<state->name.size(); i++) {
00129 last_publish_time_[state->name[i]] = state->header.stamp;
00130 }
00131 }
00132 }
00133
00134
00135
00136
00137 int main(int argc, char** argv)
00138 {
00139
00140 ros::init(argc, argv, "robot_state_publisher");
00141 NodeHandle node;
00142
00144 std::string exe_name = argv[0];
00145 std::size_t slash = exe_name.find_last_of("/");
00146 if (slash != std::string::npos) {
00147 exe_name = exe_name.substr(slash + 1);
00148 }
00149 if (exe_name == "state_publisher") {
00150 ROS_WARN("The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead");
00151 }
00153
00154
00155 urdf::Model model;
00156 model.initParam("robot_description");
00157 KDL::Tree tree;
00158 if (!kdl_parser::treeFromUrdfModel(model, tree)) {
00159 ROS_ERROR("Failed to extract kdl tree from xml robot description");
00160 return -1;
00161 }
00162
00163 MimicMap mimic;
00164
00165 for (std::map< std::string, boost::shared_ptr< urdf::Joint > >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++) {
00166 if (i->second->mimic) {
00167 mimic.insert(make_pair(i->first, i->second->mimic));
00168 }
00169 }
00170
00171 JointStateListener state_publisher(tree, mimic, model);
00172 ros::spin();
00173
00174 return 0;
00175 }