jaco_driver: jaco_tf_updater.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * jaco_tf_updater.cpp
00003 
00004  *
00005  *  Created on: Apr 16, 2013
00006  *      Author: mdedonato
00007  */
00008 
00009 #include <jaco_driver/jaco_tf_updater.h>
00010 
00011 
00012 namespace jaco
00013 {
00014 
00015 JacoTFTree::JacoTFTree(ros::NodeHandle node_handle)
00016     : kinematics_(node_handle)
00017 {
00018     joint_angles_subscriber_ = node_handle.subscribe("in/joint_angles", 1,
00019                                                      &JacoTFTree::jointAnglesMsgHandler, this);
00020     current_angles_.joint1 = 0;
00021     current_angles_.joint2 = 0;
00022     current_angles_.joint3 = 0;
00023     current_angles_.joint4 = 0;
00024     current_angles_.joint5 = 0;
00025     current_angles_.joint6 = 0;
00026     last_angle_update_ = ros::Time().now();
00027     tf_update_timer_ = node_handle.createTimer(ros::Duration(0.01),
00028                                                &JacoTFTree::tfUpdateHandler, this);
00029     tf_update_timer_.stop();
00030 }
00031 
00032 
00033 void JacoTFTree::jointAnglesMsgHandler(const jaco_msgs::JointAnglesConstPtr& joint_angles)
00034 {
00035     current_angles_.joint1 = joint_angles->joint1;
00036     current_angles_.joint2 = joint_angles->joint2;
00037     current_angles_.joint3 = joint_angles->joint3;
00038     current_angles_.joint4 = joint_angles->joint4;
00039     current_angles_.joint5 = joint_angles->joint5;
00040     current_angles_.joint6 = joint_angles->joint6;
00041     last_angle_update_ = ros::Time().now();
00042     tf_update_timer_.start();
00043 }
00044 
00045 
00046 void JacoTFTree::calculatePostion(void)
00047 {
00048     // Update the forward Kinematics
00049     kinematics_.updateForward(kinematics_.degToRad(current_angles_.joint1),
00050                               kinematics_.degToRad(current_angles_.joint2),
00051                               kinematics_.degToRad(current_angles_.joint3),
00052                               kinematics_.degToRad(current_angles_.joint4),
00053                               kinematics_.degToRad(current_angles_.joint5),
00054                               kinematics_.degToRad(current_angles_.joint6));
00055 }
00056 
00057 
00058 void JacoTFTree::tfUpdateHandler(const ros::TimerEvent&)
00059 {
00060     this->calculatePostion();  // Update TF Tree
00061 
00062     if ((ros::Time().now().toSec() - last_angle_update_.toSec()) > 1)
00063     {
00064         tf_update_timer_.stop();
00065     }
00066 }
00067 
00068 }  // namespace jaco
00069 
00070 
00071 int main(int argc, char **argv)
00072 {
00073     /* Set up ROS */
00074     ros::init(argc, argv, "jaco_tf_updater");
00075     ros::NodeHandle nh("~");
00076 
00077     jaco::JacoTFTree JacoTF(nh);
00078     ros::spin();
00079 
00080     return 0;
00081 }