6 #include "leap_motion/Human.h" 7 #include "leap_motion/Hand.h" 8 #include "leap_motion/Finger.h" 9 #include "leap_motion/Bone.h" 10 #include "leap_motion/Gesture.h" 14 #include "../inc/lmc_listener.h" 19 const std::string
fingerNames[] = {
"Thumb",
"Index",
"Middle",
"Ring",
"Pinky"};
29 header_frame_id_ =
"leap_hands";
31 enable_controller_info_ =
false;
32 enable_frame_info_ =
false;
33 enable_hand_info_ =
false;
35 enable_gesture_circle_ =
false;
36 enable_gesture_swipe_ =
false;
37 enable_gesture_screen_tap_ =
false;
38 enable_gesture_key_tap_ =
false;
50 header_frame_id_ =
"leap_hands";
52 enable_controller_info_ = args[0];
53 enable_frame_info_ = args[1];
54 enable_hand_info_ = args[2];
56 enable_gesture_circle_ = args[3];
57 enable_gesture_swipe_ = args[4];
58 enable_gesture_screen_tap_ = args[5];
59 enable_gesture_key_tap_ = args[6];
75 for (
int i = 0; i < devices.
count(); ++i)
77 ROS_INFO(
"id: %s", devices[i].toString().c_str() );
78 ROS_INFO(
" isStreaming: %s", (devices[i].isStreaming() ?
"true" :
"false") );
88 ROS_INFO(
"Circle gesture has been enabled");
98 ROS_INFO(
"Swipe gesture has been enabled");
108 ROS_INFO(
"Screen tap gesture has been enabled");
118 ROS_INFO(
"Key tap gesture has been enabled");
135 ROS_INFO(
"Listener has been initialized");
171 ROS_INFO(
"Listener object removed from the device");
196 leap_motion::Human ros_human_msg;
197 ros_human_msg.header.stamp = timestamp;
199 ros_human_msg.lmc_frame_id = frame.
id();
200 ros_human_msg.nr_of_hands = frame.
hands().count();
201 ros_human_msg.nr_of_fingers = frame.
fingers().count();
204 ros_human_msg.to_string = frame.
toString();
206 ros_human_msg.right_hand.is_present =
false;
207 ros_human_msg.left_hand.is_present =
false;
211 ROS_INFO(
"Frame id: %lu, timestamp: %f, hands: %i, fingers: %i, gestures: %i",
218 if(hand_list.count() > 2)
220 ROS_WARN(
"There are more than 2 hands in the frame! I see %i hands.", hand_list.count());
227 const Hand hand = *current_hand;
231 leap_motion::Hand ros_hand_msg;
232 ros_hand_msg.header.stamp = timestamp;
234 ros_hand_msg.lmc_hand_id = hand.
id();
235 ros_hand_msg.is_present =
true;
236 ros_hand_msg.valid_gestures =
false;
241 ros_hand_msg.pitch = hand.
direction().pitch();
242 ros_hand_msg.yaw = hand.
direction().yaw();
245 ros_hand_msg.direction.x = hand.
direction().x;
246 ros_hand_msg.direction.y = hand.
direction().y;
247 ros_hand_msg.direction.z = hand.
direction().z;
255 ros_hand_msg.palm_width = hand.
palmWidth() / 1000.0;
258 ros_hand_msg.to_string = hand.
toString();
261 ros_hand_msg.palm_velocity.push_back(hand.
palmVelocity()[0] / 1000.0);
262 ros_hand_msg.palm_velocity.push_back(hand.
palmVelocity()[1] / 1000.0);
263 ros_hand_msg.palm_velocity.push_back(hand.
palmVelocity()[2] / 1000.0);
266 ros_hand_msg.sphere_center.push_back(hand.
sphereCenter()[0] / 1000.0);
267 ros_hand_msg.sphere_center.push_back(hand.
sphereCenter()[1] / 1000.0);
268 ros_hand_msg.sphere_center.push_back(hand.
sphereCenter()[2] / 1000.0);
270 ros_hand_msg.sphere_radius = hand.
sphereRadius() / 1000.0;
274 ROS_INFO(
"%s %s, id: %i, palm position: %s mm", std::string(2,
' ').c_str(),
284 float x_basis_sign = 1.0;
291 std::vector<leap_motion::Finger> finger_msg_list;
298 const Finger finger = *fl;
300 leap_motion::Finger ros_finger_msg;
301 ros_finger_msg.header.stamp = timestamp;
303 ros_finger_msg.lmc_finger_id = finger.
id();
304 ros_finger_msg.type = finger.
type();
305 ros_finger_msg.length = finger.
length() / 1000.0;
306 ros_finger_msg.width = finger.
width() / 1000.0;
307 ros_finger_msg.to_string = finger.
toString();
311 ROS_INFO(
"%s %s, id: %i, length: %f mm, width: %f mm", std::string(4,
' ').c_str(),
317 std::vector<leap_motion::Bone> bone_msg_list;
318 for (
int b = 0; b < 4; ++b)
322 bone = finger.
bone(boneType);
324 leap_motion::Bone ros_bone_msg;
325 ros_bone_msg.
type = boneType;
326 ros_bone_msg.length = bone.length() / 1000.0;
327 ros_bone_msg.width = bone.width() / 1000.0;
328 ros_bone_msg.to_string = bone.toString();
330 ros_bone_msg.center.push_back(bone.center()[0] / 1000.0);
331 ros_bone_msg.center.push_back(bone.center()[1] / 1000.0);
332 ros_bone_msg.center.push_back(bone.center()[2] / 1000.0);
334 ros_bone_msg.bone_start.position.x = bone.prevJoint().x / 1000.0;
335 ros_bone_msg.bone_start.position.y = bone.prevJoint().y / 1000.0;
336 ros_bone_msg.bone_start.position.z = bone.prevJoint().z / 1000.0;
338 ros_bone_msg.bone_end.position.x = bone.nextJoint().x / 1000.0;
339 ros_bone_msg.bone_end.position.y = bone.nextJoint().y / 1000.0;
340 ros_bone_msg.bone_end.position.z = bone.nextJoint().z / 1000.0;
345 x_basis_sign * leap_matrix.
xBasis.
z,
350 quaternionTFToMsg(tf_quaternion, ros_bone_msg.bone_end.orientation);
351 ros_bone_msg.bone_start.orientation = ros_bone_msg.bone_end.orientation;
352 bone_msg_list.push_back(ros_bone_msg);
355 ros_finger_msg.bone_list = bone_msg_list;
356 finger_msg_list.push_back(ros_finger_msg);
359 ros_hand_msg.finger_list = finger_msg_list;
365 std::vector<leap_motion::Gesture> gesture_msg_list;
366 ros_hand_msg.valid_gestures =
true;
373 for(
unsigned int i = 0; i < hands_for_gesture.
count(); i++)
375 if(*current_hand == hands_for_gesture[i])
377 leap_motion::Gesture ros_gesture_msg;
378 ros_gesture_msg.lmc_gesture_id = (*gl).id();
379 ros_gesture_msg.duration_us = (*gl).duration();
380 ros_gesture_msg.duration_s = (*gl).durationSeconds();
381 ros_gesture_msg.is_valid = (*gl).isValid();
382 ros_gesture_msg.gesture_state = (*gl).state();
383 ros_gesture_msg.gesture_type = (*gl).type();
384 ros_gesture_msg.to_string = (*gl).toString();
388 std::vector<int32_t> pointable_ids;
389 for(
unsigned int j = 0; j < pointables_for_gesture.
count(); j++)
391 pointable_ids.push_back( pointables_for_gesture[j].
id() );
394 ros_gesture_msg.pointable_ids = pointable_ids;
395 gesture_msg_list.push_back(ros_gesture_msg);
399 ros_hand_msg.gesture_list = gesture_msg_list;
403 ros_hand_msg.arm.header.stamp = timestamp;
405 ros_hand_msg.arm.elbow.position.x = hand.
arm().elbowPosition()[0] / 1000.0;
406 ros_hand_msg.arm.elbow.position.y = hand.
arm().elbowPosition()[1] / 1000.0;
407 ros_hand_msg.arm.elbow.position.z = hand.
arm().elbowPosition()[2] / 1000.0;
409 ros_hand_msg.arm.center.push_back(hand.
arm().center()[0] / 1000.0);
410 ros_hand_msg.arm.center.push_back(hand.
arm().center()[1] / 1000.0);
411 ros_hand_msg.arm.center.push_back(hand.
arm().center()[2] / 1000.0);
413 ros_hand_msg.arm.wrist.position.x = hand.
arm().wristPosition()[0] / 1000.0;
414 ros_hand_msg.arm.wrist.position.y = hand.
arm().wristPosition()[1] / 1000.0;
415 ros_hand_msg.arm.wrist.position.z = hand.
arm().wristPosition()[2] / 1000.0;
417 ros_hand_msg.arm.direction.x = hand.
arm().center()[0] / 1000.0;
418 ros_hand_msg.arm.direction.y = hand.
arm().center()[1] / 1000.0;
419 ros_hand_msg.arm.direction.z = hand.
arm().center()[2] / 1000.0;
424 x_basis_sign * leap_matrix.
xBasis.
z,
429 quaternionTFToMsg(tf_quaternion, ros_hand_msg.arm.elbow.orientation);
430 ros_hand_msg.arm.wrist.orientation = ros_hand_msg.arm.elbow.orientation;
434 ros_human_msg.right_hand = ros_hand_msg;
438 ros_human_msg.left_hand = ros_hand_msg;
441 ros_publisher.publish(ros_human_msg);
LEAP_EXPORT void enableGesture(Gesture::Type type, bool enable=true) const
LEAP_EXPORT const_iterator begin() const
LEAP_EXPORT bool isRight() const
bool enable_gesture_swipe_
LEAP_EXPORT float grabStrength() const
LEAP_EXPORT Vector sphereCenter() const
LEAP_EXPORT Vector palmNormal() const
LEAP_EXPORT GestureList gestures() const
bool enable_gesture_screen_tap_
LEAP_EXPORT float pinchStrength() const
LEAP_EXPORT float currentFramesPerSecond() const
std::string toString() const
void getRotation(Quaternion &q) const
LEAP_EXPORT int count() const
virtual void onInit(const Controller &)
Called once, when this Listener object is newly added to a Controller.
virtual void onFrame(const Controller &)
Called when a new frame of hand and finger tracking data is available.
std::string toString() const
LEAP_EXPORT Vector palmPosition() const
LEAP_EXPORT float palmWidth() const
LEAP_EXPORT Bone bone(Bone::Type boneIx) const
LEAP_EXPORT Vector palmVelocity() const
LeapListener()
Uses default values for initial setup.
LEAP_EXPORT const_iterator end() const
std::string toString() const
bool enable_gesture_circle_
std::string header_frame_id_
virtual void onDisconnect(const Controller &)
Called when the Controller object disconnects.
bool enable_controller_info_
virtual void onExit(const Controller &)
Called when this Listener object is removed from the Controller.
LEAP_EXPORT int count() const
std::string toString() const
LEAP_EXPORT int count() const
virtual void onConnect(const Controller &)
Called when a connection is established with the controller.
const std::string fingerNames[]
LEAP_EXPORT float timeVisible() const
LEAP_EXPORT Frame frame(int history=0) const
LEAP_EXPORT const_iterator end() const
LEAP_EXPORT float sphereRadius() const
bool enable_gesture_key_tap_
LEAP_EXPORT bool isEmpty() const
LEAP_EXPORT int count() const
LEAP_EXPORT const_iterator begin() const