$search
00001 00027 #include "sr_tactile_sensors/sr_gazebo_virtual_tactile_sensor.hpp" 00028 #include <boost/algorithm/string.hpp> 00029 #include <string> 00030 #include <math.h> 00031 #include <ros/ros.h> 00032 00033 namespace shadowrobot 00034 { 00035 /********************************** 00036 * TACTILE SENSOR * 00037 **********************************/ 00038 SrGazeboVirtualTactileSensor::SrGazeboVirtualTactileSensor(std::string name, 00039 std::string gazebo_bumper_topic) : 00040 SrGenericTactileSensor(name, ""), 00041 touch_value(0.0), 00042 touch_freshdata(false) 00043 { 00044 sub = nh.subscribe(gazebo_bumper_topic, 2, &SrGazeboVirtualTactileSensor::callback, this); 00045 } 00046 00047 SrGazeboVirtualTactileSensor::~SrGazeboVirtualTactileSensor() 00048 {} 00049 00050 void SrGazeboVirtualTactileSensor::callback(const gazebo_plugins::ContactsState& msg) 00051 { 00052 double tmp_value; 00053 ::geometry_msgs::Vector3 v; 00054 v.x=0; 00055 v.y=0; 00056 v.z=0; 00057 00058 ROS_DEBUG("Touch by %s", msg.header.frame_id.c_str()); 00059 // Parse the message to retrieve the relevant touch pressure information 00060 // Currently the sum of all contacts. 00061 // More sophisticated analysis can be done to take the contact that is the most aligned with the distal link normal 00062 00063 if(msg.states.size()>0) 00064 { 00065 unsigned int nb_wrench = msg.states[0].wrenches.size(); 00066 for (unsigned int i=0;i<nb_wrench;i++) 00067 { 00068 v.x=v.x+msg.states[0].wrenches[i].force.x; 00069 v.y=v.y+msg.states[0].wrenches[i].force.y; 00070 v.z=v.z+msg.states[0].wrenches[i].force.z; 00071 } 00072 v.x=v.x/nb_wrench; 00073 v.y=v.y/nb_wrench; 00074 v.z=v.z/nb_wrench; 00075 tmp_value = sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2)); 00076 touch_mutex.lock(); 00077 touch_value = tmp_value; 00078 touch_freshdata = true; 00079 touch_mutex.unlock(); 00080 } 00081 } 00082 00083 double SrGazeboVirtualTactileSensor::get_touch_data() 00084 { 00085 double return_value; 00086 touch_mutex.lock(); 00087 if(touch_freshdata) 00088 { 00089 return_value = touch_value; 00090 touch_freshdata=false; 00091 } 00092 else 00093 { 00094 return_value= 0.0; 00095 } 00096 touch_mutex.unlock(); 00097 00098 return return_value; 00099 } 00100 00101 00102 /********************************** 00103 * TACTILE SENSOR MANAGER * 00104 **********************************/ 00105 SrGazeboVirtualTactileSensorManager::SrGazeboVirtualTactileSensorManager() : 00106 SrTactileSensorManager() 00107 { 00108 std::vector<std::vector<std::string> > all_names = get_all_names(); 00109 00110 for( unsigned int i=0; i< all_names[0].size() ; ++i) 00111 { 00112 tactile_sensors.push_back( 00113 boost::shared_ptr<SrGazeboVirtualTactileSensor>( 00114 new SrGazeboVirtualTactileSensor(all_names[0][i], 00115 "/"+all_names[0][i]+"distal_bumper/state") 00116 ) 00117 ); 00118 } 00119 } 00120 00121 SrGazeboVirtualTactileSensorManager::~SrGazeboVirtualTactileSensorManager() 00122 {} 00123 } 00124 00125 00134 int main(int argc, char** argv) 00135 { 00136 ros::init(argc, argv, "sr_tactile_sensor"); 00137 ros::NodeHandle n; 00138 00139 shadowrobot::SrGazeboVirtualTactileSensorManager tact_sens_mgr; 00140 00141 while( ros::ok() ) 00142 tact_sens_mgr.publish_all(); 00143 00144 return 0; 00145 } 00146 00147 /* For the emacs weenies in the crowd. 00148 Local Variables: 00149 c-basic-offset: 2 00150 End: 00151 */