2 #include "leap_motion/Human.h" 3 #include "leap_motion/Hand.h" 4 #include "leap_motion/Finger.h" 5 #include "leap_motion/Bone.h" 9 #include "../inc/lmc_filter_node.h" 13 std::vector< std::vector<lpf> >
hands;
79 double lpf::filter(
const double& new_msrmt,
int b_idx,
int dim)
94 return new_filtered_msrmt;
118 hands[hand_idx][5].setInitialPos( hand.palm_center.x, 0 , 0);
119 hands[hand_idx][5].setInitialPos( hand.palm_center.y, 0 , 1);
120 hands[hand_idx][5].setInitialPos( hand.palm_center.z, 0 , 2);
123 hand.palm_center.x =
hands[hand_idx][5].filter( hand.palm_center.x, 0 , 0);
124 hand.palm_center.y =
hands[hand_idx][5].filter( hand.palm_center.y, 0 , 1);
125 hand.palm_center.z =
hands[hand_idx][5].filter( hand.palm_center.z, 0 , 2);
127 leap_motion::Finger finger;
128 for(
unsigned int j = 0; j < hand.finger_list.size(); j++)
130 finger = hand.finger_list[j];
131 leap_motion::Bone bone;
132 for(
unsigned int k = 0; k < finger.bone_list.size(); k++)
134 bone = finger.bone_list[k];
137 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.x, bone.type, 0);
138 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.y, bone.type, 1);
139 hands[hand_idx][finger.type].setInitialPos(bone.bone_end.position.z, bone.type, 2);
142 bone.bone_end.position.x =
hands[hand_idx][finger.type].filter(bone.bone_end.position.x, bone.type, 0);
143 bone.bone_end.position.y =
hands[hand_idx][finger.type].filter(bone.bone_end.position.y, bone.type, 1);
144 bone.bone_end.position.z =
hands[hand_idx][finger.type].filter(bone.bone_end.position.z, bone.type, 2);
148 bone.bone_start.position.x =
hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 0);
149 bone.bone_start.position.y =
hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 1);
150 bone.bone_start.position.z =
hands[hand_idx][finger.type].getFilteredMsrmt( bone.type - 1, 2);
156 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.x, 5, 0);
157 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.y, 5, 1);
158 hands[hand_idx][finger.type].setInitialPos(bone.bone_start.position.z, 5, 2);
160 bone.bone_start.position.x =
hands[hand_idx][finger.type].filter(bone.bone_start.position.x, 5, 0);
161 bone.bone_start.position.y =
hands[hand_idx][finger.type].filter(bone.bone_start.position.y, 5, 1);
162 bone.bone_start.position.z =
hands[hand_idx][finger.type].filter(bone.bone_start.position.z, 5, 2);
186 leap_motion::Hand hand;
188 if( human -> right_hand.is_present )
190 hand = human -> right_hand;
194 if( human -> left_hand.is_present )
196 hand = human -> left_hand;
204 int main(
int argc,
char** argv) {
210 nh.
getParam(
"/filter_cutoff", cutoff);
211 nh.
getParam(
"/enable_filter", is_enabled);
216 hands.push_back( std::vector<lpf> ( 6,
lpf( cutoff ) ) );
217 hands.push_back( std::vector<lpf> ( 6,
lpf( cutoff ) ) );
void setInitialPos(double initial_msrmt, int b_idx, int dim)
This is used to set the inital position values of a filter.
void publish(const boost::shared_ptr< M > &message) const
static ros::Publisher * filt_pub
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
void messageCallback(const leap_motion::Human::ConstPtr &human)
A callback function that is run everytime a new Human.msg is received.
int main(int argc, char **argv)
std::vector< std::vector< lpf > > hands
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
lpf()
Uses default values for initial setup.
double getFilteredMsrmt(int f_idx, int dim)
This is used to get a filtered value of a bone.
void setCutoff(double)
Use this to change the cutoff of a lpf object.
bool getParam(const std::string &key, std::string &s) const
double prev_msrmts_[6][3][3]
double filter(const double &new_msrmt, int f_idx, int dim)
This is used to filter the position values of a bone.
void filterMessage(leap_motion::Hand hand, uint8_t hand_idx)
This is used to filter the unfiltered human.msg.
double prev_filtered_msrmts_[6][3][2]