Go to the documentation of this file.00001 #include "ros/ros.h"
00002 #include <tf/transform_datatypes.h>
00003
00004 #include "leap_motion/Human.h"
00005 #include "leap_motion/Hand.h"
00006 #include "leap_motion/Finger.h"
00007 #include "leap_motion/Bone.h"
00008 #include "leap_motion/Gesture.h"
00009
00010 #include <iostream>
00011
00012 #include "../inc/lmc_listener.h"
00013 #include "Leap.h"
00014
00015 using namespace Leap;
00016
00017 const std::string fingerNames[] = {"Thumb", "Index", "Middle", "Ring", "Pinky"};
00018
00026 LeapListener::LeapListener(){
00027 header_frame_id_ = "leap_hands";
00028
00029 enable_controller_info_ = false;
00030 enable_frame_info_ = false;
00031 enable_hand_info_ = false;
00032
00033 enable_gesture_circle_ = false;
00034 enable_gesture_swipe_ = false;
00035 enable_gesture_screen_tap_ = false;
00036 enable_gesture_key_tap_ = false;
00037 }
00038
00047 LeapListener::LeapListener(bool args[7]){
00048 header_frame_id_ = "leap_hands";
00049
00050 enable_controller_info_ = args[0];
00051 enable_frame_info_ = args[1];
00052 enable_hand_info_ = args[2];
00053
00054 enable_gesture_circle_ = args[3];
00055 enable_gesture_swipe_ = args[4];
00056 enable_gesture_screen_tap_ = args[5];
00057 enable_gesture_key_tap_ = args[6];
00058 }
00059
00068 void LeapListener::onConnect(const Controller& controller)
00069 {
00070 if(LeapListener::enable_controller_info_)
00071 {
00072 const DeviceList devices = controller.devices();
00073 for (int i = 0; i < devices.count(); ++i)
00074 {
00075 ROS_INFO( "id: %s", devices[i].toString().c_str() );
00076 ROS_INFO(" isStreaming: %s", (devices[i].isStreaming() ? "true" : "false") );
00077 }
00078 }
00079
00080
00081 if(LeapListener::enable_gesture_circle_)
00082 {
00083 controller.enableGesture(Leap::Gesture::TYPE_CIRCLE);
00084 if(LeapListener::enable_controller_info_)
00085 {
00086 ROS_INFO("Circle gesture has been enabled");
00087 }
00088 }
00089
00090
00091 if(LeapListener::enable_gesture_swipe_)
00092 {
00093 controller.enableGesture(Leap::Gesture::TYPE_SWIPE);
00094 if(LeapListener::enable_controller_info_)
00095 {
00096 ROS_INFO("Swipe gesture has been enabled");
00097 }
00098 }
00099
00100
00101 if(LeapListener::enable_gesture_screen_tap_)
00102 {
00103 controller.enableGesture(Leap::Gesture::TYPE_SCREEN_TAP);
00104 if(LeapListener::enable_controller_info_)
00105 {
00106 ROS_INFO("Screen tap gesture has been enabled");
00107 }
00108 }
00109
00110
00111 if(LeapListener::enable_gesture_key_tap_)
00112 {
00113 controller.enableGesture(Leap::Gesture::TYPE_KEY_TAP);
00114 if(LeapListener::enable_controller_info_)
00115 {
00116 ROS_INFO("Key tap gesture has been enabled");
00117 }
00118 }
00119 };
00120
00129 void LeapListener::onInit(const Controller& controller)
00130 {
00131 if(LeapListener::enable_controller_info_)
00132 {
00133 ROS_INFO("Listener has been initialized");
00134 }
00135 };
00136
00149 void LeapListener::onDisconnect(const Controller& controller)
00150 {
00151 if(LeapListener::enable_controller_info_)
00152 {
00153 ROS_INFO("Device disconnected");
00154 }
00155 };
00156
00165 void LeapListener::onExit(const Controller& controller)
00166 {
00167 if(LeapListener::enable_controller_info_)
00168 {
00169 ROS_INFO("Listener object removed from the device");
00170 }
00171 };
00172
00187 void LeapListener::onFrame(const Controller& controller)
00188 {
00189 const Frame frame = controller.frame();
00190 ros::Time timestamp = ros::Time::now();
00191
00192
00193
00194 leap_motion::Human ros_human_msg;
00195 ros_human_msg.header.stamp = timestamp;
00196 ros_human_msg.header.frame_id = LeapListener::header_frame_id_;
00197 ros_human_msg.lmc_frame_id = frame.id();
00198 ros_human_msg.nr_of_hands = frame.hands().count();
00199 ros_human_msg.nr_of_fingers = frame.fingers().count();
00200 ros_human_msg.nr_of_gestures = frame.gestures().count();
00201 ros_human_msg.current_frames_per_second = frame.currentFramesPerSecond();
00202 ros_human_msg.to_string = frame.toString();
00203
00204 ros_human_msg.right_hand.is_present = false;
00205 ros_human_msg.left_hand.is_present = false;
00206
00207 if(LeapListener::enable_frame_info_)
00208 {
00209 ROS_INFO("Frame id: %lu, timestamp: %f, hands: %i, fingers: %i, gestures: %i",
00210 frame.id(), timestamp.toSec(), frame.hands().count(), frame.fingers().count(),
00211 frame.gestures().count() );
00212 }
00213
00214
00215 HandList hand_list = frame.hands();
00216 if(hand_list.count() > 2)
00217 {
00218 ROS_WARN("There are more than 2 hands in the frame! I see %i hands.", hand_list.count());
00219 }
00220 else
00221 {
00222 for (HandList::const_iterator current_hand = hand_list.begin(); current_hand != hand_list.end(); ++current_hand)
00223 {
00224
00225 const Hand hand = *current_hand;
00226
00227
00228
00229 leap_motion::Hand ros_hand_msg;
00230 ros_hand_msg.header.stamp = timestamp;
00231 ros_hand_msg.header.frame_id = LeapListener::header_frame_id_;
00232 ros_hand_msg.lmc_hand_id = hand.id();
00233 ros_hand_msg.is_present = true;
00234 ros_hand_msg.valid_gestures = false;
00235 ros_hand_msg.confidence = hand.confidence();
00236
00237
00238
00239
00240 ros_hand_msg.roll = hand.direction().pitch();
00241 ros_hand_msg.pitch = hand.direction().yaw();
00242 ros_hand_msg.yaw = hand.palmNormal().roll();
00243
00244 ros_hand_msg.grab_strength = hand.grabStrength();
00245 ros_hand_msg.palm_width = hand.palmWidth() / 1000.0;
00246 ros_hand_msg.pinch_strength = hand.pinchStrength();
00247 ros_hand_msg.time_visible = hand.timeVisible();
00248 ros_hand_msg.to_string = hand.toString();
00249
00250
00251 ros_hand_msg.palm_velocity.push_back(hand.palmVelocity()[0] / 1000.0);
00252 ros_hand_msg.palm_velocity.push_back(hand.palmVelocity()[1] / 1000.0);
00253 ros_hand_msg.palm_velocity.push_back(hand.palmVelocity()[2] / 1000.0);
00254
00255
00256 ros_hand_msg.sphere_center.push_back(hand.sphereCenter()[0] / 1000.0);
00257 ros_hand_msg.sphere_center.push_back(hand.sphereCenter()[1] / 1000.0);
00258 ros_hand_msg.sphere_center.push_back(hand.sphereCenter()[2] / 1000.0);
00259
00260 ros_hand_msg.sphere_radius = hand.sphereRadius() / 1000.0;
00261
00262
00263 ros_hand_msg.wrist_position.push_back(hand.wristPosition()[0] / 1000.0);
00264 ros_hand_msg.wrist_position.push_back(hand.wristPosition()[1] / 1000.0);
00265 ros_hand_msg.wrist_position.push_back(hand.wristPosition()[2] / 1000.0);
00266
00267 if(LeapListener::enable_hand_info_)
00268 {
00269 ROS_INFO("%s %s, id: %i, palm position: %s mm", std::string(2, ' ').c_str(),
00270 (hand.isRight() ? "Right hand" : "Left hand"), hand.id(), hand.palmPosition().toString().c_str() );
00271 }
00272
00273
00274 ros_hand_msg.palm_center.x = hand.palmPosition().x / 1000.0;
00275 ros_hand_msg.palm_center.y = hand.palmPosition().y / 1000.0;
00276 ros_hand_msg.palm_center.z = hand.palmPosition().z / 1000.0;
00277
00278
00279 std::vector<leap_motion::Finger> finger_msg_list;
00280
00281
00282 const FingerList fingers = hand.fingers();
00283 for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl)
00284 {
00285
00286 const Finger finger = *fl;
00287
00288 leap_motion::Finger ros_finger_msg;
00289 ros_finger_msg.header.stamp = timestamp;
00290 ros_finger_msg.header.frame_id = LeapListener::header_frame_id_;
00291 ros_finger_msg.lmc_finger_id = finger.id();
00292 ros_finger_msg.type = finger.type();
00293 ros_finger_msg.length = finger.length() / 1000.0;
00294 ros_finger_msg.width = finger.width() / 1000.0;
00295 ros_finger_msg.to_string = finger.toString();
00296
00297 if(LeapListener::enable_hand_info_)
00298 {
00299 ROS_INFO("%s %s, id: %i, length: %f mm, width: %f mm", std::string(4, ' ').c_str(),
00300 fingerNames[finger.type()].c_str(), finger.id(), finger.length(), finger.width());
00301 }
00302
00303 Bone bone;
00304 Bone::Type boneType;
00305 std::vector<leap_motion::Bone> bone_msg_list;
00306 for (int b = 0; b < 4; ++b)
00307 {
00308
00309 boneType = static_cast<Bone::Type>(b);
00310 bone = finger.bone(boneType);
00311
00312 leap_motion::Bone ros_bone_msg;
00313 ros_bone_msg.type = boneType;
00314 ros_bone_msg.length = bone.length() / 1000.0;
00315 ros_bone_msg.width = bone.width() / 1000.0;
00316 ros_bone_msg.to_string = bone.toString();
00317
00318 ros_bone_msg.center.push_back(bone.center()[0] / 1000.0);
00319 ros_bone_msg.center.push_back(bone.center()[1] / 1000.0);
00320 ros_bone_msg.center.push_back(bone.center()[2] / 1000.0);
00321
00322 ros_bone_msg.bone_start.position.x = bone.prevJoint().x / 1000.0;
00323 ros_bone_msg.bone_start.position.y = bone.prevJoint().y / 1000.0;
00324 ros_bone_msg.bone_start.position.z = bone.prevJoint().z / 1000.0;
00325
00326 ros_bone_msg.bone_end.position.x = bone.nextJoint().x / 1000.0;
00327 ros_bone_msg.bone_end.position.y = bone.nextJoint().y / 1000.0;
00328 ros_bone_msg.bone_end.position.z = bone.nextJoint().z / 1000.0;
00329
00330 bone_msg_list.push_back(ros_bone_msg);
00331 }
00332
00333 ros_finger_msg.bone_list = bone_msg_list;
00334 finger_msg_list.push_back(ros_finger_msg);
00335 }
00336
00337 ros_hand_msg.finger_list = finger_msg_list;
00338
00339
00340
00341 if (!frame.gestures().isEmpty())
00342 {
00343 std::vector<leap_motion::Gesture> gesture_msg_list;
00344 ros_hand_msg.valid_gestures = true;
00345
00346 Leap::GestureList gestures = frame.gestures();
00347 for(Leap::GestureList::const_iterator gl = gestures.begin(); gl != gestures.end(); gl++)
00348 {
00349
00350 Leap::HandList hands_for_gesture = (*gl).hands();
00351 for(unsigned int i = 0; i < hands_for_gesture.count(); i++)
00352 {
00353 if(*current_hand == hands_for_gesture[i])
00354 {
00355 leap_motion::Gesture ros_gesture_msg;
00356 ros_gesture_msg.lmc_gesture_id = (*gl).id();
00357 ros_gesture_msg.duration_us = (*gl).duration();
00358 ros_gesture_msg.duration_s = (*gl).durationSeconds();
00359 ros_gesture_msg.is_valid = (*gl).isValid();
00360 ros_gesture_msg.gesture_state = (*gl).state();
00361 ros_gesture_msg.gesture_type = (*gl).type();
00362 ros_gesture_msg.to_string = (*gl).toString();
00363
00364
00365 Leap::PointableList pointables_for_gesture = (*gl).pointables();
00366 std::vector<int32_t> pointable_ids;
00367 for(unsigned int j = 0; j < pointables_for_gesture.count(); j++)
00368 {
00369 pointable_ids.push_back( pointables_for_gesture[j].id() );
00370 }
00371
00372 ros_gesture_msg.pointable_ids = pointable_ids;
00373 gesture_msg_list.push_back(ros_gesture_msg);
00374 }
00375 }
00376 }
00377 ros_hand_msg.gesture_list = gesture_msg_list;
00378 }
00379
00380 if( hand.isRight() )
00381 {
00382 ros_human_msg.right_hand = ros_hand_msg;
00383 }
00384 else
00385 {
00386 ros_human_msg.left_hand = ros_hand_msg;
00387 }
00388 }
00389 ros_publisher.publish(ros_human_msg);
00390 }
00391 };