00001 /* 00002 * jaco_tf_updater.h 00003 * 00004 * Created on: Apr 16, 2013 00005 * Author: mdedonato 00006 */ 00007 00008 #ifndef JACO_DRIVER_JACO_TF_UPDATER_H 00009 #define JACO_DRIVER_JACO_TF_UPDATER_H 00010 00011 #include <time.h> 00012 00013 #include <jaco_driver/jaco_arm_kinematics.h> 00014 00015 #include <ros/ros.h> 00016 #include <std_msgs/String.h> 00017 #include <tf/tf.h> 00018 #include <tf/transform_broadcaster.h> 00019 #include <tf/transform_listener.h> 00020 00021 #include "jaco_msgs/JointAngles.h" 00022 00023 00024 namespace jaco 00025 { 00026 00027 class JacoTFTree 00028 { 00029 public: 00030 explicit JacoTFTree(ros::NodeHandle nh); 00031 00032 private: 00033 void jointAnglesMsgHandler(const jaco_msgs::JointAnglesConstPtr& joint_angles); 00034 void calculatePostion(void); 00035 void tfUpdateHandler(const ros::TimerEvent&); 00036 00037 jaco::JacoKinematics kinematics_; 00038 jaco_msgs::JointAngles current_angles_; 00039 ros::Time last_angle_update_; 00040 ros::Subscriber joint_angles_subscriber_; 00041 ros::Timer tf_update_timer_; 00042 }; 00043 00044 } // namespace jaco 00045 #endif // JACO_DRIVER_JACO_TF_UPDATER_H