00001 00034 #ifndef RIDGEBACK_BASE_PASSIVE_JOINT_PUBLISHER_H 00035 #define RIDGEBACK_BASE_PASSIVE_JOINT_PUBLISHER_H 00036 00037 #include "ros/ros.h" 00038 #include "sensor_msgs/JointState.h" 00039 00040 namespace ridgeback_base 00041 { 00042 00043 class PassiveJointPublisher 00044 { 00045 public: 00046 PassiveJointPublisher(ros::NodeHandle& nh, ros::V_string& joints, int frequency) 00047 { 00048 for (int i = 0; i < joints.size(); i++) 00049 { 00050 msg_.name.push_back(joints[i]); 00051 msg_.position.push_back(0); 00052 msg_.velocity.push_back(0); 00053 msg_.effort.push_back(0); 00054 } 00055 pub_ = nh.advertise<sensor_msgs::JointState>("/joint_states", 1); 00056 timer_ = nh.createTimer(ros::Duration(1.0/frequency), &PassiveJointPublisher::timerCb, this); 00057 } 00058 00059 void timerCb(const ros::TimerEvent&) 00060 { 00061 msg_.header.stamp = ros::Time::now(); 00062 pub_.publish(msg_); 00063 } 00064 00065 private: 00066 sensor_msgs::JointState msg_; 00067 ros::Publisher pub_; 00068 ros::Timer timer_; 00069 }; 00070 00071 } // namespace ridgeback_base 00072 00073 #endif // RIDGEBACK_BASE_PASSIVE_JOINT_PUBLISHER_H