leap_camera.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 "sensor_msgs/Image.h"
00006 #include "sensor_msgs/CameraInfo.h"
00007 #include "camera_info_manager/camera_info_manager.h"
00008 #include "rospack/rospack.h"
00009 
00010 #include <boost/shared_ptr.hpp>
00011 #include <sstream>
00012 
00013 
00014 #define targetWidth 500
00015 #define targetHeight 500
00016 #define cutWidth 280
00017 #define cutHeight 220
00018 #define startX 110
00019 #define startY 140
00020 
00021 using namespace Leap;
00022 using namespace std;
00023 
00024 
00025 
00026 class CameraListener : public Listener {
00027   public:
00028   //ros::NodeHandle _node;
00029   boost::shared_ptr <ros::NodeHandle> _left_node;
00030   boost::shared_ptr <ros::NodeHandle> _right_node;
00031   
00032   ros::Publisher _pub_image_left;
00033   ros::Publisher _pub_info_left;
00034   ros::Publisher _pub_image_right;
00035   ros::Publisher _pub_info_right;
00036   camera_info_manager::CameraInfoManager* info_mgr_right;
00037   camera_info_manager::CameraInfoManager* info_mgr_left;
00038   unsigned int seq;
00039   virtual void onInit(const Controller&);
00040   virtual void onConnect(const Controller&);
00041   virtual void onDisconnect(const Controller&);
00042   virtual void onExit(const Controller&);
00043   virtual void onFrame(const Controller&);
00044   virtual void onFocusGained(const Controller&);
00045   virtual void onFocusLost(const Controller&);
00046   virtual void onDeviceChange(const Controller&);
00047   virtual void onServiceConnect(const Controller&);
00048   virtual void onServiceDisconnect(const Controller&);  
00049   private:
00050 };
00051 
00052 
00053 void CameraListener::onInit(const Controller& controller){
00054 
00055   _left_node = boost::make_shared<ros::NodeHandle>("left");
00056   _right_node = boost::make_shared<ros::NodeHandle>("right");
00057   std::cout << "Initialized" << std::endl;
00058   _pub_image_left = _left_node->advertise<sensor_msgs::Image>("image_raw", 1);
00059   _pub_info_left = _left_node->advertise<sensor_msgs::CameraInfo>("camera_info", 1);
00060   _pub_image_right = _right_node->advertise<sensor_msgs::Image>("image_raw", 1);
00061   _pub_info_right = _right_node->advertise<sensor_msgs::CameraInfo>("camera_info", 1);
00062   seq = 0;
00063   std::string default_l_info_filename;
00064   std::string default_r_info_filename;
00065   rospack::Rospack rp;
00066   std::vector<std::string> search_path;
00067   rp.getSearchPathFromEnv(search_path);
00068   rp.crawl(search_path, 1);
00069   std::string path;
00070   if (rp.find("leap_motion",path)==true) {
00071     default_l_info_filename = path + std::string("/camera_info/leap_cal_left.yml");
00072     default_r_info_filename = path + std::string("/camera_info/leap_cal_right.yml");
00073   }
00074   else {
00075     default_l_info_filename = "";
00076     default_r_info_filename = "";
00077   }
00078   ros::NodeHandle local_nh("~");
00079   std::string l_info_filename;
00080   std::string r_info_filename;
00081   local_nh.param("template_filename_left", l_info_filename, default_l_info_filename);
00082   local_nh.param("template_filename_right", r_info_filename, default_r_info_filename);
00083   l_info_filename = std::string("file://") + l_info_filename;
00084   r_info_filename = std::string("file://") + r_info_filename;
00085   info_mgr_left = new camera_info_manager::CameraInfoManager(*_left_node, "left", l_info_filename);
00086   info_mgr_right = new camera_info_manager::CameraInfoManager(*_right_node, "right", r_info_filename);
00087 }
00088 
00089 void CameraListener::onConnect(const Controller& controller) {
00090   std::cout << "Connected" << std::endl;
00091   controller.enableGesture(Gesture::TYPE_CIRCLE);
00092   controller.enableGesture(Gesture::TYPE_KEY_TAP);
00093   controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
00094   controller.enableGesture(Gesture::TYPE_SWIPE);
00095 }
00096 
00097 void CameraListener::onDisconnect(const Controller& controller) {
00098   // Note: not dispatched when running in a debugger.
00099   std::cout << "Disconnected" << std::endl;
00100 }
00101 
00102 void CameraListener::onExit(const Controller& controller) {
00103   std::cout << "Exited" << std::endl;
00104 }
00105 
00106 void CameraListener::onFrame(const Controller& controller) {
00107   // Get the most recent frame and report some basic information
00108   const Frame frame = controller.frame();  
00109   ImageList images = frame.images();
00110 
00111   sensor_msgs::Image image_msg;
00112   image_msg.header.seq = seq++;
00113   image_msg.header.stamp =ros::Time::now();
00114   image_msg.encoding ="mono8";
00115   image_msg.is_bigendian = 0;
00116   for(int camera_num=0; camera_num<2; camera_num++){
00117     Image image = images[camera_num];
00118     image_msg.width =  cutWidth;
00119     image_msg.height = cutHeight;
00120     image_msg.step = cutWidth;
00121     image_msg.data.resize(cutWidth*cutHeight);
00122 #pragma omp parallel for
00123     for(int i=0; i<cutWidth; i++){
00124       for(int j=0; j<cutHeight; j++){
00125         Vector input = Vector((float)(i+startX)/targetWidth, (float)(j+startY)/targetHeight, 0);
00126         input.x = (input.x - image.rayOffsetX()) / image.rayScaleX();
00127         input.y = (input.y - image.rayOffsetY()) / image.rayScaleY();
00128         Vector pixel = image.warp(input);
00129         if(pixel.x >= 0 && pixel.x < image.width() && pixel.y >= 0 && pixel.y < image.height()) {
00130           int data_index = floor(pixel.y) * image.width() + floor(pixel.x);
00131           image_msg.data[cutWidth*j+i] = image.data()[data_index];
00132         } else {
00133           image_msg.data[cutWidth*j+i] = 0;
00134         }
00135       }
00136     }
00137     if(camera_num==0){
00138       sensor_msgs::CameraInfoPtr info_msg(new sensor_msgs::CameraInfo(info_mgr_left->getCameraInfo()));
00139       image_msg.header.frame_id = info_msg->header.frame_id ="leap_optical_frame";
00140       info_msg->width = image_msg.width;
00141       info_msg->height = image_msg.height;
00142       info_msg->header.stamp = image_msg.header.stamp;
00143       info_msg->header.seq = image_msg.header.seq;
00144       _pub_image_left.publish(image_msg);
00145       _pub_info_left.publish(*info_msg);
00146     }else{
00147       sensor_msgs::CameraInfoPtr info_msg(new sensor_msgs::CameraInfo(info_mgr_right->getCameraInfo()));
00148       image_msg.header.frame_id = info_msg->header.frame_id = "leap_optical_frame";
00149       info_msg->width = image_msg.width;
00150       info_msg->height = image_msg.height;
00151       info_msg->header.stamp = image_msg.header.stamp;
00152       info_msg->header.seq = image_msg.header.seq;
00153       _pub_image_right.publish(image_msg);
00154       _pub_info_right.publish(*info_msg);
00155     }
00156   }
00157   // test
00158 
00159   //to do
00160   // int start_x = 100;
00161   // int start_y = 100;
00162   // int end_x = 300;
00163   // int end_y = 300;
00164   // int width_x = end_x - start_x;
00165   // int width_y = end_y - start_y;
00166   //     } else {
00167   //    image_msg.data[width_x*(j-start_y)+(i-start_x)] = 255;
00168   //     }
00169   //   }
00170   // }
00171   //end for test
00172 }
00173 
00174 void CameraListener::onFocusGained(const Controller& controller) {
00175   std::cout << "Focus Gained" << std::endl;
00176 }
00177 
00178 void CameraListener::onFocusLost(const Controller& controller) {
00179   std::cout << "Focus Lost" << std::endl;
00180 }
00181 
00182 void CameraListener::onDeviceChange(const Controller& controller) {
00183   std::cout << "Device Changed" << std::endl;
00184   const DeviceList devices = controller.devices();
00185 
00186   for (int i = 0; i < devices.count(); ++i) {
00187     std::cout << "id: " << devices[i].toString() << std::endl;
00188     std::cout << "  isStreaming: " << (devices[i].isStreaming() ? "true" : "false") << std::endl;
00189   }
00190 }
00191 
00192 void CameraListener::onServiceConnect(const Controller& controller) {
00193   std::cout << "Service Connected" << std::endl;
00194 }
00195 
00196 void CameraListener::onServiceDisconnect(const Controller& controller) {
00197   std::cout << "Service Disconnected" << std::endl;
00198 }
00199 
00200 int main(int argc, char** argv) {
00201   ros::init(argc, argv, "leap_sender");
00202   // Create a sample listener and controller
00203   CameraListener listener;
00204   Controller controller;
00205 
00206   
00207   
00208   // Have the sample listener receive events from the controller
00209   controller.addListener(listener);
00210   
00211   controller.setPolicyFlags(static_cast<Leap::Controller::PolicyFlag> (Leap::Controller::POLICY_IMAGES));
00212   ros::spin();
00213   // Remove the sample listener when done
00214   controller.removeListener(listener);
00215 
00216   return 0;
00217 }


leap_motion
Author(s): Florian Lier
autogenerated on Thu Aug 27 2015 13:50:42