$search
00001 00029 #include <ros/ros.h> 00030 #include <string> 00031 00032 #include <boost/thread.hpp> 00033 00034 //messages 00035 #include <std_msgs/Float64.h> 00036 00037 //a ros subscriber (will be instantiated later on) 00038 ros::Subscriber sub[5]; 00039 std_msgs::Float64::_data_type data[5]; 00040 boost::mutex update_mutex; 00041 00042 00043 void callback_ff(const std_msgs::Float64ConstPtr& msg) 00044 { 00045 update_mutex.lock(); 00046 data[0] = msg->data; 00047 update_mutex.unlock(); 00048 } 00049 void callback_mf(const std_msgs::Float64ConstPtr& msg) 00050 { 00051 update_mutex.lock(); 00052 data[1] = msg->data; 00053 update_mutex.unlock(); 00054 } 00055 void callback_rf(const std_msgs::Float64ConstPtr& msg) 00056 { 00057 update_mutex.lock(); 00058 data[2] = msg->data; 00059 update_mutex.unlock(); 00060 } 00061 void callback_lf(const std_msgs::Float64ConstPtr& msg) 00062 { 00063 update_mutex.lock(); 00064 data[3] = msg->data; 00065 update_mutex.unlock(); 00066 } 00067 void callback_th(const std_msgs::Float64ConstPtr& msg) 00068 { 00069 update_mutex.lock(); 00070 data[4] = msg->data; 00071 update_mutex.unlock(); 00072 } 00073 00074 00075 int main(int argc, char** argv) 00076 { 00077 //init the ros node 00078 ros::init(argc, argv, "test_tactile"); 00079 00080 ros::NodeHandle node_tactile; 00081 00082 sub[0] = node_tactile.subscribe("/sr_tactile/touch/ff", 2, callback_ff); 00083 sub[1] = node_tactile.subscribe("/sr_tactile/touch/mf", 2, callback_mf); 00084 sub[2] = node_tactile.subscribe("/sr_tactile/touch/rf", 2, callback_rf); 00085 sub[3] = node_tactile.subscribe("/sr_tactile/touch/lf", 2, callback_lf); 00086 sub[4] = node_tactile.subscribe("/sr_tactile/touch/th", 2, callback_th); 00087 00088 ros::Rate publish_rate = ros::Rate(20.0); 00089 std_msgs::Float64::_data_type cur_data[5] = {0}; 00090 00091 while( ros::ok() ) 00092 { 00093 update_mutex.lock(); 00094 for (int i=0; i<5; i++) 00095 cur_data[i] = data[i]; 00096 update_mutex.unlock(); 00097 00098 ROS_ERROR("TACTILE SENSOR READING: %f %f %f %f %f", 00099 cur_data[0], 00100 cur_data[1], 00101 cur_data[2], 00102 cur_data[3], 00103 cur_data[4]); 00104 00105 publish_rate.sleep(); 00106 ros::spinOnce(); 00107 } 00108 00109 return 0; 00110 }