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 : n_tilde_("~"), publish_rate_(0.0), state_publisher_(tree)
00052 {
00053
00054 double publish_freq;
00055 n_tilde_.param("publish_frequency", publish_freq, 50.0);
00056 publish_rate_ = Rate(publish_freq);
00057
00058 if (tree.getNrOfJoints() == 0){
00059 boost::shared_ptr<sensor_msgs::JointState> empty_state(new sensor_msgs::JointState);
00060 while (ros::ok()){
00061 empty_state->header.stamp = ros::Time::now();
00062 this->callbackJointState(empty_state);
00063 publish_rate_.sleep();
00064 }
00065 }
00066 else{
00067
00068 joint_state_sub_ = n_.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
00069 }
00070 };
00071
00072
00073 JointStateListener::~JointStateListener()
00074 {};
00075
00076
00077 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
00078 {
00079 if (state->name.size() != state->position.size()){
00080 ROS_ERROR("Robot state publisher received an invalid joint state vector");
00081 return;
00082 }
00083
00084
00085 map<string, double> joint_positions;
00086 for (unsigned int i=0; i<state->name.size(); i++)
00087 joint_positions.insert(make_pair(state->name[i], state->position[i]));
00088 state_publisher_.publishTransforms(joint_positions, state->header.stamp);
00089 publish_rate_.sleep();
00090 }
00091
00092
00093
00094
00095
00096
00097
00098
00099 int main(int argc, char** argv)
00100 {
00101
00102 ros::init(argc, argv, "robot_state_publisher");
00103 NodeHandle node;
00104
00105
00106 KDL::Tree tree;
00107 if (!kdl_parser::treeFromParam("robot_description", tree)){
00108 ROS_ERROR("Failed to extract kdl tree from xml robot description");
00109 return -1;
00110 }
00111
00112 if (tree.getNrOfSegments() == 0){
00113 ROS_WARN("Robot state publisher got an empty tree and cannot publish any state to tf");
00114 ros::spin();
00115 }
00116 else{
00117 JointStateListener state_publisher(tree);
00118 ros::spin();
00119 }
00120
00121 return 0;
00122 }