leap_hands.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <string.h>
00003 #include "Leap.h"
00004 #include "ros/ros.h"
00005 #include "visualization_msgs/MarkerArray.h"
00006 #include "geometry_msgs/PoseStamped.h"
00007 
00008 #include <sstream>
00009 
00010 
00011 using namespace Leap;
00012 using namespace std;
00013 
00014 class HandsListener : public Listener {
00015   public:
00016   ros::NodeHandle _node;
00017   ros::Publisher _pub_marker_array;
00018   ros::Publisher _pub_bone_only;
00019   unsigned int seq;
00020   virtual void onInit(const Controller&);
00021   virtual void onConnect(const Controller&);
00022   virtual void onDisconnect(const Controller&);
00023   virtual void onExit(const Controller&);
00024   virtual void onFrame(const Controller&);
00025   virtual void onFocusGained(const Controller&);
00026   virtual void onFocusLost(const Controller&);
00027   virtual void onDeviceChange(const Controller&);
00028   virtual void onServiceConnect(const Controller&);
00029   virtual void onServiceDisconnect(const Controller&);  
00030   private:
00031 };
00032 
00033 void HandsListener::onInit(const Controller& controller) {
00034   std::cout << "Initialized" << std::endl;
00035   _pub_marker_array = _node.advertise<visualization_msgs::MarkerArray>("hands", 1);
00036   _pub_bone_only = _node.advertise<visualization_msgs::Marker>("hands_line", 1);
00037 }
00038 
00039 
00040 void HandsListener::onConnect(const Controller& controller) {
00041   std::cout << "Connected" << std::endl;
00042   controller.enableGesture(Gesture::TYPE_CIRCLE);
00043   controller.enableGesture(Gesture::TYPE_KEY_TAP);
00044   controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
00045   controller.enableGesture(Gesture::TYPE_SWIPE);
00046 }
00047 
00048 void HandsListener::onDisconnect(const Controller& controller) {
00049   // Note: not dispatched when running in a debugger.
00050   std::cout << "Disconnected" << std::endl;
00051 }
00052 
00053 void HandsListener::onExit(const Controller& controller) {
00054   std::cout << "Exited" << std::endl;
00055 }
00056 
00057 void HandsListener::onFrame(const Controller& controller) {
00058   // Get the most recent frame and report some basic information
00059   const Frame frame = controller.frame();
00060   //ROS_INFO("flags = %i", (int) controller.policyFlags());
00061   visualization_msgs::Marker marker_msg, joint_msg;
00062   visualization_msgs::MarkerArray marker_array_msg;
00063   marker_msg.header.frame_id=joint_msg.header.frame_id="/leap_optical_frame";
00064   marker_msg.header.stamp=joint_msg.header.stamp=ros::Time::now();
00065   marker_msg.ns="leap_marker";
00066   joint_msg.ns="joint";
00067   marker_msg.id = 0; 
00068   joint_msg.id = 0;
00069   marker_msg.type = visualization_msgs::Marker::LINE_LIST;
00070   joint_msg.type = visualization_msgs::Marker::SPHERE;
00071   marker_msg.scale.x = 0.005;
00072   joint_msg.scale.x = joint_msg.scale.y = joint_msg.scale.z = 0.015;
00073   joint_msg.color.r = .0f;
00074   joint_msg.color.g = 1.0f;
00075   joint_msg.color.b = 1.0f;
00076   joint_msg.color.a = 0.7f;
00077   marker_msg.action = joint_msg.action = visualization_msgs::Marker::ADD;
00078   marker_msg.lifetime = joint_msg.lifetime = ros::Duration(0.1);
00079 
00080   HandList hands = frame.hands();
00081   for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
00082     // Get the first hand
00083     const Hand hand = *hl;
00084     // Get the Arm bone
00085     // Get fingers
00086     const FingerList fingers = hand.fingers();
00087     for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
00088       const Finger finger = *fl;
00089       // Get finger bones
00090       for (int b = 0; b < 4; ++b) {
00091         Bone::Type boneType = static_cast<Bone::Type>(b);
00092         Bone bone = finger.bone(boneType);
00093         geometry_msgs::Point point;
00094         point.x = -bone.prevJoint().x/1000;
00095         point.y = bone.prevJoint().z/1000;
00096         point.z = bone.prevJoint().y/1000;
00097         marker_msg.points.push_back(point);
00098         point.x = joint_msg.pose.position.x =  -bone.nextJoint().x/1000;
00099         point.y = joint_msg.pose.position.y = bone.nextJoint().z/1000;
00100         point.z = joint_msg.pose.position.z = bone.nextJoint().y/1000;
00101         marker_msg.points.push_back(point);
00102         joint_msg.id = joint_msg.id+1;
00103         marker_array_msg.markers.push_back(joint_msg);
00104         std_msgs::ColorRGBA color;
00105         color.r = 1.0f; color.g=.0f; color.b=.0f, color.a=1.0f;
00106         marker_msg.colors.push_back(color);
00107         marker_msg.colors.push_back(color);
00108       }
00109     }
00110   }
00111   _pub_marker_array.publish(marker_array_msg);
00112   _pub_bone_only.publish(marker_msg);
00113 }
00114 
00115 void HandsListener::onFocusGained(const Controller& controller) {
00116   std::cout << "Focus Gained" << std::endl;
00117 }
00118 
00119 void HandsListener::onFocusLost(const Controller& controller) {
00120   std::cout << "Focus Lost" << std::endl;
00121 }
00122 
00123 void HandsListener::onDeviceChange(const Controller& controller) {
00124   std::cout << "Device Changed" << std::endl;
00125   const DeviceList devices = controller.devices();
00126 
00127   for (int i = 0; i < devices.count(); ++i) {
00128     std::cout << "id: " << devices[i].toString() << std::endl;
00129     std::cout << "  isStreaming: " << (devices[i].isStreaming() ? "true" : "false") << std::endl;
00130   }
00131 }
00132 
00133 void HandsListener::onServiceConnect(const Controller& controller) {
00134   std::cout << "Service Connected" << std::endl;
00135 }
00136 
00137 void HandsListener::onServiceDisconnect(const Controller& controller) {
00138   std::cout << "Service Disconnected" << std::endl;
00139 }
00140 
00141 int main(int argc, char** argv) {
00142   ros::init(argc, argv, "leap_sender");
00143   // Create a sample listener and controller
00144   HandsListener listener;
00145   Controller controller;
00146   
00147   // Have the sample listener receive events from the controller
00148   controller.addListener(listener);
00149 
00150   controller.setPolicyFlags(static_cast<Leap::Controller::PolicyFlag> (Leap::Controller::POLICY_IMAGES));
00151   ros::spin();
00152   // Remove the sample listener when done
00153   controller.removeListener(listener);
00154 
00155   return 0;
00156 }


leap_motion
Author(s): Florian Lier
autogenerated on Tue Jan 17 2017 21:06:16