00001 #include <ros/ros.h> 00002 #include <std_msgs/Float64.h> 00003 #include <strstream> 00004 #include <sstream> 00005 #include <exception> 00006 #include <boost/thread.hpp> 00007 #include <sensor_msgs/JointState.h> 00008 00009 #include "image_transport/subscriber.h" 00010 #include "sensor_msgs/Image.h" 00011 #include <image_transport/image_transport.h> 00012 #include <opencv2/highgui/highgui.hpp> 00013 #include <cv_bridge/cv_bridge.h> 00014 #include "opencv2/imgproc/imgproc.hpp" 00015 #include <sensor_msgs/image_encodings.h> 00016 #include <std_msgs/Float64MultiArray.h> 00017 00018 #define JOINTS_NUM 6 00019 #define KERNEL_SIZE 4 00020 #define DOF 4 00021 00022 #define CAMERA_DATA_SIZE 20 00023 00024 #define LAMBDA_X 200 00025 #define LAMBDA_Y 200 00026 #define LAMBDA_Z 200 00027 #define LAMBDA_THETA -0.01 00028 00029 #define L1 0.3 00030 #define L2 0.25 00031 #define L3 0.16 00032 #define L5 0.072 00033 #define SAMPLE_TIME 0.01 00034 #define JOINTS_ERROR 0.01 00035 class VisualServoController 00036 { 00037 public: 00038 VisualServoController(); 00039 ~VisualServoController(); 00040 bool Spin(); 00041 /*set joints positions (in radian and m)*/ 00042 bool setJointPositions(float target_positions[JOINTS_NUM]); 00043 float* getJointPositions(); 00044 void setTargetPositions(float target_positions[JOINTS_NUM]); 00045 void setInitialPositions(float target_positions[JOINTS_NUM]); 00046 void hardSetJointPosition(float target_positions[JOINTS_NUM]); 00047 void executeControlAlgorithm(); 00048 00049 private: 00050 bool camera_call_backed_; 00051 00052 void initializeSubscribers(); 00053 void initializePublishers(); 00054 00055 /* callback function for getting joint states */ 00056 void jointStateCB(const sensor_msgs::JointStatePtr& joint_states); 00057 void cameraDataCB(const std_msgs::Float64MultiArray::ConstPtr &camera_data_arr); 00058 00059 00060 void inverseKinematic(float target_joints[JOINTS_NUM], float x, float y, float z,float a); 00061 void forwardKinematic(float target_joints[JOINTS_NUM], float &x, float &y, float &z, float &a); 00062 00063 float joints_position_[JOINTS_NUM]; 00064 float target_joints_position_[JOINTS_NUM]; 00065 00066 image_transport::Subscriber threshold_image_sub_; 00067 00068 double target_kernel_[KERNEL_SIZE]; 00069 00070 cv::Mat jacobian_inverse_mat_; 00071 double kernel_[KERNEL_SIZE]; 00072 00073 ros::NodeHandle nh_; 00074 ros::Publisher joint_pub_[JOINTS_NUM]; 00075 ros::Subscriber joint_state_sub_; 00076 ros::Subscriber camera_data_sub_; 00077 00078 00079 };