00001 #ifndef VISUALSERVOCAMERA_H 00002 #define VISUALSERVOCAMERA_H 00003 00004 #include <ros/ros.h> 00005 #include <std_msgs/Float64.h> 00006 #include <strstream> 00007 #include <sstream> 00008 #include <exception> 00009 00010 #include "image_transport/subscriber.h" 00011 #include "sensor_msgs/Image.h" 00012 #include <image_transport/image_transport.h> 00013 #include <opencv2/highgui/highgui.hpp> 00014 #include <cv_bridge/cv_bridge.h> 00015 #include "opencv2/imgproc/imgproc.hpp" 00016 #include <sensor_msgs/image_encodings.h> 00017 #include <std_msgs/Float64MultiArray.h> 00018 00019 00020 #define MAX_BINARY_VALUE 255.0 00021 #define THRESHOLD_VALUE 250.0 00022 00023 #define DELTA_X 0.017 00024 #define DELTA_Y 0.0185 00025 00026 #define KERNEL_P -2 00027 #define KERNEL_Q -2 00028 00029 #define KERNEL_SIZE 4 00030 #define DOF 4 00031 00032 00033 class VisualServoCamera 00034 { 00035 public: 00036 VisualServoCamera(); 00037 ~VisualServoCamera(); 00038 private: 00039 00040 00041 ros::NodeHandle nh_; 00042 void imageCB(const sensor_msgs::ImageConstPtr& image_msg); 00043 00044 cv::Mat cur_image_; //current image 00045 cv::Mat grey_image_, color_image_,threshold_image_; 00046 image_transport::Subscriber image_sub_; 00047 image_transport::Publisher threshold_image_pub_; 00048 cv_bridge::CvImage threshold_image_msg_;// thresholded image 00049 00050 ros::Publisher camera_data_pub_; 00051 00052 // Kernel gets image and computes kesi, Ger and Jacobian 00053 bool calculateKernel(const cv::Mat *image,float kernel[KERNEL_SIZE],cv::Mat &jacobian_mat , cv::Mat &jacobian_inverse_mat); 00054 void publishCameraData(const float kernel[], const cv::Mat jacobian_inverse_mat); 00055 00056 00057 00058 00059 }; 00060 00061 #endif