00001 #include "ros/ros.h"
00002 #include "leap_motion/Human.h"
00003 #include "leap_motion/Hand.h"
00004 #include "leap_motion/Finger.h"
00005 #include "leap_motion/Bone.h"
00006
00007 #include <iostream>
00008 #include "Leap.h"
00009 #include "../inc/lmc_filter_node.h"
00010
00011 static ros::Publisher* filt_pub;
00012
00013 std::vector< std::vector<lpf> > hands;
00014 bool set_up_right = true;
00015 bool set_up_left = true;
00016
00023 lpf::lpf()
00024 {
00025 c_ = 4.0;
00026 }
00027
00035 lpf::lpf(double cutoff)
00036 {
00037 c_ = cutoff;
00038 }
00039
00047 void lpf::setCutoff(double cutoff)
00048 {
00049 c_ = cutoff;
00050 }
00051
00059 void lpf::setInitialPos(double initial_msrmt, int b_idx, int dim)
00060 {
00061 prev_msrmts_[b_idx][dim][0] = initial_msrmt;
00062 prev_msrmts_[b_idx][dim][1] = initial_msrmt;
00063 prev_msrmts_[b_idx][dim][2] = initial_msrmt;
00064
00065 prev_filtered_msrmts_[b_idx][dim][0] = initial_msrmt;
00066 prev_filtered_msrmts_[b_idx][dim][1] = initial_msrmt;
00067 }
00068
00079 double lpf::filter(const double& new_msrmt, int b_idx, int dim)
00080 {
00081
00082 prev_msrmts_[b_idx][dim][2] = prev_msrmts_[b_idx][dim][1];
00083 prev_msrmts_[b_idx][dim][1] = prev_msrmts_[b_idx][dim][0];
00084 prev_msrmts_[b_idx][dim][0] = new_msrmt;
00085
00086 double new_filtered_msrmt = (1 / (1 + c_*c_ + 1.414*c_) ) * (prev_msrmts_[b_idx][dim][2] + 2*prev_msrmts_[b_idx][dim][1]
00087 + prev_msrmts_[b_idx][dim][0] - (c_*c_ - 1.414*c_ + 1) * prev_filtered_msrmts_[b_idx][dim][1]
00088 - (-2 * c_*c_ + 2) * prev_filtered_msrmts_[b_idx][dim][0]);
00089
00090
00091 prev_filtered_msrmts_[b_idx][dim][1] = prev_filtered_msrmts_[b_idx][dim][0];
00092 prev_filtered_msrmts_[b_idx][dim][0] = new_filtered_msrmt;
00093
00094 return new_filtered_msrmt;
00095 }
00096
00103 double lpf::getFilteredMsrmt(int b_idx, int dim){
00104 return prev_filtered_msrmts_[b_idx][dim][0];
00105 }
00106
00116 void filterMessage(leap_motion::Hand hand, uint8_t hand_idx){
00117 if(set_up_right || set_up_left ){
00118 hands[hand_idx][5].setInitialPos( hand.palm_center.x, 0 , 0);
00119 hands[hand_idx][5].setInitialPos( hand.palm_center.y, 0 , 1);
00120 hands[hand_idx][5].setInitialPos( hand.palm_center.z, 0 , 2);
00121 }
00122
00123 hand.palm_center.x = hands[hand_idx][5].filter( hand.palm_center.x, 0 , 0);
00124 hand.palm_center.y = hands[hand_idx][5].filter( hand.palm_center.y, 0 , 1);
00125 hand.palm_center.z = hands[hand_idx][5].filter( hand.palm_center.z, 0 , 2);
00126
00127 leap_motion::Finger finger;
00128 for(unsigned int j = 0; j < hand.finger_list.size(); j++)
00129 {
00130 finger = hand.finger_list[j];
00131 leap_motion::Bone bone;
00132 for(unsigned int k = 0; k < finger.bone_list.size(); k++)
00133 {
00134 bone = finger.bone_list[k];
00135 if(set_up_right || set_up_left)
00136 {
00137 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.x, bone.type, 0);
00138 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.y, bone.type, 1);
00139 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.z, bone.type, 2);
00140 }
00141
00142 bone.bone_end.position.x = hands[hand_idx][finger.type].filter(bone.bone_end.position.x, bone.type, 0);
00143 bone.bone_end.position.y = hands[hand_idx][finger.type].filter(bone.bone_end.position.y, bone.type, 1);
00144 bone.bone_end.position.z = hands[hand_idx][finger.type].filter(bone.bone_end.position.z, bone.type, 2);
00145
00146 if(bone.type != 0)
00147 {
00148 bone.bone_start.position.x = hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 0);
00149 bone.bone_start.position.y = hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 1);
00150 bone.bone_start.position.z = hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 2);
00151 }
00152 else
00153 {
00154 if( set_up_right || set_up_left )
00155 {
00156 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.x, 5, 0);
00157 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.y, 5, 1);
00158 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.z, 5, 2);
00159 }
00160 bone.bone_start.position.x = hands[hand_idx][finger.type].filter(bone.bone_start.position.x, 5, 0);
00161 bone.bone_start.position.y = hands[hand_idx][finger.type].filter(bone.bone_start.position.y, 5, 1);
00162 bone.bone_start.position.z = hands[hand_idx][finger.type].filter(bone.bone_start.position.z, 5, 2);
00163 }
00164 }
00165 }
00166 if(set_up_right)
00167 {
00168 set_up_right = false;
00169 }
00170 if(set_up_left)
00171 {
00172 set_up_left = false;
00173 }
00174 }
00175
00185 void messageCallback(const leap_motion::Human::ConstPtr& human){
00186 leap_motion::Hand hand;
00187
00188 if( human -> right_hand.is_present )
00189 {
00190 hand = human -> right_hand;
00191 filterMessage(hand, 0);
00192 }
00193
00194 if( human -> left_hand.is_present )
00195 {
00196 hand = human -> left_hand;
00197 filterMessage(hand, 1);
00198 }
00199
00200
00201 filt_pub->publish(human);
00202 }
00203
00204 int main(int argc, char** argv) {
00205 ros::init(argc, argv, "leap_motion");
00206 ros::NodeHandle nh("leap_motion");
00207
00208 double cutoff;
00209 bool is_enabled;
00210 nh.getParam("/filter_cutoff", cutoff);
00211 nh.getParam("/enable_filter", is_enabled);
00212
00213 if(is_enabled)
00214 {
00215
00216 hands.push_back( std::vector<lpf> ( 6, lpf( cutoff ) ) );
00217 hands.push_back( std::vector<lpf> ( 6, lpf( cutoff ) ) );
00218
00219 ros::Subscriber human_sub = nh.subscribe<leap_motion::Human>("leap_device", 1, messageCallback);
00220 ros::Publisher f_pub = nh.advertise<leap_motion::Human>("leap_filtered", 1);
00221 filt_pub = &f_pub;
00222
00223 ros::spin();
00224 }
00225
00226 return 0;
00227 }