$search
00001 00028 #include "sr_tactile_sensors/sr_generic_tactile_sensor.hpp" 00029 00030 namespace shadowrobot 00031 { 00032 /********************************** 00033 * TACTILE SENSOR * 00034 **********************************/ 00035 SrGenericTactileSensor::SrGenericTactileSensor(std::string name, 00036 std::string touch_name ) : 00037 n_tilde("~") 00038 { 00039 std::string full_topic_touch = "touch/"+name; 00040 00041 touch_pub = n_tilde.advertise<std_msgs::Float64>(full_topic_touch, 10); 00042 } 00043 00044 SrGenericTactileSensor::~SrGenericTactileSensor() 00045 {} 00046 00047 void SrGenericTactileSensor::publish_current_values() 00048 { 00049 msg_touch.data = get_touch_data(); 00050 00051 touch_pub.publish(msg_touch); 00052 } 00053 00054 /********************************** 00055 * TACTILE SENSOR MANAGER * 00056 **********************************/ 00057 SrTactileSensorManager::SrTactileSensorManager() : 00058 n_tilde("~"), publish_rate(20.0) 00059 { 00060 using namespace XmlRpc; 00061 double publish_freq; 00062 n_tilde.param("publish_frequency", publish_freq, 20.0); 00063 publish_rate = ros::Rate(publish_freq); 00064 00065 //initializing the thresholds to test if the hand is holding 00066 //something or not (compared agains the pressure value). 00067 double tmp[5]={117,117,113,111,0}; 00068 XmlRpc::XmlRpcValue threshold_xmlrpc; 00069 if(n_tilde.getParam("/grasp_touch_thresholds",threshold_xmlrpc)) 00070 { 00071 ROS_ASSERT(threshold_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray); 00072 if(threshold_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) 00073 { 00074 if(threshold_xmlrpc.size() == 5) 00075 { 00076 for (int i = 0; i < threshold_xmlrpc.size(); ++i) 00077 { 00078 if(threshold_xmlrpc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble) 00079 tmp[i] = static_cast<double>(threshold_xmlrpc[i]); 00080 else 00081 ROS_ERROR("grasp_touch_thresholds value %d is not a double, not loading",i+1); 00082 } 00083 } 00084 else 00085 ROS_ERROR("grasp_touch_thresholds must be of size 5, using default values"); 00086 } 00087 else 00088 ROS_ERROR("grasp_touch_thresholds must be an array, using default values"); 00089 } 00090 else 00091 ROS_WARN("grasp_touch_thresholds not set, using default values"); 00092 00093 is_hand_occupied_thresholds = std::vector<double>(tmp, tmp+5); 00094 ROS_DEBUG("is_hand_occupied_thresholds:[%f %f %f %f %f]", is_hand_occupied_thresholds.at(0),is_hand_occupied_thresholds.at(1),is_hand_occupied_thresholds.at(2),is_hand_occupied_thresholds.at(3),is_hand_occupied_thresholds.at(4)); 00095 00096 00097 is_hand_occupied_server = n_tilde.advertiseService("is_hand_occupied", &SrTactileSensorManager::is_hand_occupied_cb, this); 00098 which_fingers_are_touching_server = n_tilde.advertiseService("which_fingers_are_touching", &SrTactileSensorManager::which_fingers_are_touching_cb, this); 00099 } 00100 00101 SrTactileSensorManager::~SrTactileSensorManager() 00102 {} 00103 00104 bool SrTactileSensorManager::is_hand_occupied_cb(sr_robot_msgs::is_hand_occupied::Request &req, 00105 sr_robot_msgs::is_hand_occupied::Response &res ) 00106 { 00107 bool is_occupied = true; 00108 00109 for(unsigned int i=0; i < tactile_sensors.size(); ++i) 00110 { 00111 if(tactile_sensors[i]->get_touch_data() < is_hand_occupied_thresholds[i]) 00112 { 00113 is_occupied = false; 00114 ROS_DEBUG("is_hand_occupied_thresholds %d with val %f is smaller than threshold %f",i,tactile_sensors[i]->get_touch_data(),is_hand_occupied_thresholds[i]); 00115 break; 00116 } 00117 } 00118 00119 res.hand_occupied = is_occupied; 00120 00121 return true; 00122 } 00123 00124 bool SrTactileSensorManager::which_fingers_are_touching_cb(sr_robot_msgs::which_fingers_are_touching::Request &req, 00125 sr_robot_msgs::which_fingers_are_touching::Response &res ) 00126 { 00127 std::vector<double> touch_values(5); 00128 ROS_ASSERT(tactile_sensors.size() == 5); 00129 00130 double value_tmp = 0.0; 00131 for(unsigned int i=0; i < tactile_sensors.size(); ++i) 00132 { 00133 value_tmp = tactile_sensors[i]->get_touch_data(); 00134 if(value_tmp < req.force_thresholds[i]) 00135 touch_values[i] = 0.0; 00136 else 00137 touch_values[i] = value_tmp; 00138 } 00139 res.touch_forces = touch_values; 00140 return true; 00141 } 00142 00143 std::vector<std::vector<std::string> > SrTactileSensorManager::get_all_names() 00144 { 00145 std::vector<std::vector<std::string> > all_names_vector; 00146 std::vector<std::string> names, sensor_touch_names; 00147 00148 std::string tmp; 00149 XmlRpc::XmlRpcValue my_list; 00150 int list_size; 00151 bool bad_params = false; 00152 00153 n_tilde.getParam("display_names", my_list); 00154 if(my_list.getType() != XmlRpc::XmlRpcValue::TypeArray) 00155 bad_params = true; 00156 list_size = my_list.size(); 00157 for (int32_t i = 0; i < list_size; ++i) 00158 { 00159 if(my_list[i].getType() != XmlRpc::XmlRpcValue::TypeString) 00160 bad_params = true; 00161 names.push_back( static_cast<std::string>(my_list[i]) ); 00162 } 00163 00164 n_tilde.getParam("sensor_touch_names", my_list); 00165 if(my_list.getType() != XmlRpc::XmlRpcValue::TypeArray) 00166 bad_params = true; 00167 if(my_list.size() != list_size) 00168 bad_params = true; 00169 list_size = my_list.size(); 00170 for (int32_t i = 0; i < list_size; ++i) 00171 { 00172 if(my_list[i].getType() != XmlRpc::XmlRpcValue::TypeString) 00173 bad_params = true; 00174 sensor_touch_names.push_back( static_cast<std::string>(my_list[i]) ); 00175 } 00176 00177 if(bad_params) 00178 { 00179 ROS_ERROR("Error while reading the parameter for the tactile sensors; using standard parameters"); 00180 names.clear(); 00181 sensor_touch_names.clear(); 00182 00183 names.push_back("ff"); 00184 names.push_back("mf"); 00185 names.push_back("rf"); 00186 names.push_back("lf"); 00187 names.push_back("th"); 00188 00189 sensor_touch_names.push_back("FF_Touch"); 00190 sensor_touch_names.push_back("MF_Touch"); 00191 sensor_touch_names.push_back("RF_Touch"); 00192 sensor_touch_names.push_back("LF_Touch"); 00193 sensor_touch_names.push_back("TH_Touch"); 00194 } 00195 00196 all_names_vector.push_back(names); 00197 all_names_vector.push_back(sensor_touch_names); 00198 00199 return all_names_vector; 00200 } 00201 00202 void SrTactileSensorManager::publish_all() 00203 { 00204 for(unsigned int i=0; i < tactile_sensors.size(); ++i) 00205 tactile_sensors[i]->publish_current_values(); 00206 00207 publish_rate.sleep(); 00208 ros::spinOnce(); 00209 } 00210 } 00211 00212 00213 /* For the emacs weenies in the crowd. 00214 Local Variables: 00215 c-basic-offset: 2 00216 End: 00217 */