$search
00001 /* 00002 * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu> 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 00031 #include <ias_drawer_executive/Pressure.h> 00032 00033 00034 void Pressure::calcCenter(double pressure[], double &xcenter, double &ycenter) 00035 { 00036 double xsum = 0; 00037 double ysum = 0; 00038 double psum = 0; 00039 for (int i = 7; i <= 21; ++i) 00040 { 00041 double x = ((i - 7) % 3) - 1; 00042 double y = (((i - 7) - ((i - 7) % 3)) / 3) - 2; 00043 //ROS_INFO("COORDS idx %i, x %f, y %f", i, x, y); 00044 xsum += x * pressure[i]; 00045 ysum += y * pressure[i]; 00046 psum += pressure[i]; 00047 } 00048 if (psum < 5000) 00049 psum = 5000; 00050 00051 if (psum != 0) 00052 { 00053 xsum /= psum; 00054 ysum /= psum; 00055 } 00056 xcenter = xsum; 00057 ycenter = ysum; 00058 } 00059 00060 00061 void Pressure::pressureCallback(const pr2_msgs::PressureState::ConstPtr& msg) 00062 { 00063 if (zero_cnt_ <= 10) 00064 { 00065 zero_cnt_ ++; 00066 for (int i = 0; i < 22; i++) 00067 { 00068 r_sum[i] += msg->r_finger_tip[i]; 00069 l_sum[i] += msg->l_finger_tip[i]; 00070 } 00071 if (zero_cnt_ == 10) 00072 { 00073 pressure_mutex.lock(); 00074 for (int i = 0; i < 22; i++) 00075 { 00076 r_zero[i] = r_sum[i] / zero_cnt_; 00077 l_zero[i] = l_sum[i] / zero_cnt_; 00078 //ROS_INFO("ZERO OF GRIPPER PRESSURE i %i r %f l %f", i, r_zero[i], l_zero[i]); 00079 } 00080 pressure_mutex.unlock(); 00081 } 00082 } 00083 00084 double r[2]; 00085 double l[2]; 00086 double r_act[22]; 00087 double l_act[22]; 00088 for (int i = 0; i < 22; i++) 00089 { 00090 r_act[i] = msg->r_finger_tip[i] - r_zero[i]; 00091 l_act[i] = msg->l_finger_tip[i] - l_zero[i]; 00092 } 00093 00094 calcCenter(r_act, r[0], r[1]); 00095 calcCenter(l_act, l[0], l[1]); 00096 //ROS_INFO("CENTERS rx %f ry %f , lx %f ly %f", r[0], r[1], l[0], l[1]); 00097 00098 //ROS_INFO("DIFFS x %f y %f" , r[0] - l[0], r[1] - l[1]); 00099 00100 pressure_mutex.lock(); 00101 r_center[0] = r[0]; 00102 r_center[1] = r[1]; 00103 l_center[0] = l[0]; 00104 l_center[1] = l[1]; 00105 for (int i = 0; i < 22; i++) 00106 { 00107 r_curr[i] = msg->r_finger_tip[i]; 00108 l_curr[i] = msg->l_finger_tip[i]; 00109 } 00110 pressure_mutex.unlock(); 00111 00112 for (int finger_tip = 0; finger_tip < 2; ++finger_tip) 00113 { 00114 for (int nrSensor = 0; nrSensor < 22; ++nrSensor) 00115 { 00116 //if ((touched[finger_tip][nrSensor]==1) && (val[finger_tip][nrSensor]>=val_prev[finger_tip][nrSensor])) 00117 //slope[finger_tip][nrSensor]=1; 00118 int val = (finger_tip ? l_curr[nrSensor] : r_curr[nrSensor]); 00119 00120 slope[finger_tip][nrSensor]=(double)(val-val_prev_prev[finger_tip][nrSensor])/val_prev_prev[finger_tip][nrSensor]; 00121 00122 val_prev_prev[finger_tip][nrSensor]=val_prev[finger_tip][nrSensor]; 00123 val_prev[finger_tip][nrSensor]=val; 00124 00125 if (slope[finger_tip][nrSensor]>0.05) { 00126 touched[finger_tip][nrSensor]++; 00127 } 00128 } 00129 } 00130 initialized = true; 00131 } 00132 00133 Pressure::Pressure(int side) 00134 { 00135 sub_ = n_.subscribe((side == 0) ? "/pressure/r_gripper_motor" : "/pressure/l_gripper_motor", 1000, &Pressure::pressureCallback,this); // zzz 00136 zero_cnt_ = 0; 00137 side_ = side; 00138 for (int i = 0; i < 22; i++) 00139 { 00140 r_sum[i] = 0; 00141 l_sum[i] = 0; 00142 r_zero[i] = 0; 00143 l_zero[i] = 0; 00144 val_prev[0][i] = 0; 00145 val_prev[1][i] = 0; 00146 val_prev_prev[0][i] = 0; 00147 val_prev_prev[1][i] = 0; 00148 touched[0][i] = 0; 00149 touched[1][i] = 0; 00150 } 00151 initialized = false; 00152 } 00153 00154 Pressure::~Pressure() 00155 { 00156 //delete sub_; 00157 } 00158 00159 Pressure *Pressure::instance_[] = {0,0}; 00160 00161 Pressure *Pressure::getInstance(int side) 00162 { 00163 if (!instance_[side]) 00164 { 00165 instance_[side] = new Pressure(side); 00166 } 00167 return instance_[side]; 00168 } 00169 00170 void Pressure::reset() 00171 { 00172 pressure_mutex.lock(); 00173 zero_cnt_ = 0; 00174 pressure_mutex.unlock(); 00175 ros::Rate rate(25.0); 00176 00177 long act_cnt = 0; 00178 while (act_cnt < 10) 00179 { 00180 rate.sleep(); 00181 ros::spinOnce(); 00182 pressure_mutex.lock(); 00183 if (act_cnt < zero_cnt_) 00184 ROS_INFO("Waiting for zeroing of pressure sensors"); 00185 act_cnt = zero_cnt_; 00186 pressure_mutex.unlock(); 00187 } 00188 00189 } 00190 00191 void Pressure::getCenter(double *r, double *l) 00192 { 00193 pressure_mutex.lock(); 00194 r[0] = r_center[0]; 00195 r[1] = r_center[1]; 00196 l[0] = l_center[0]; 00197 l[1] = l_center[1]; 00198 pressure_mutex.unlock(); 00199 } 00200 00201 void Pressure::getCurrent(double r[], double l[], bool zero) 00202 { 00203 ros::spinOnce(); 00204 pressure_mutex.lock(); 00205 for (int i = 0; i < 22; i++) 00206 { 00207 if (zero) 00208 { 00209 r[i] = r_curr[i] - r_zero[i]; 00210 l[i] = l_curr[i] - l_zero[i]; 00211 } 00212 else 00213 { 00214 r[i] = r_curr[i]; 00215 l[i] = l_curr[i]; 00216 } 00217 } 00218 pressure_mutex.unlock(); 00219 } 00220 00221 00222 void Pressure::getInside(double &r, double &l, bool zero) 00223 { 00224 r = 0; 00225 l = 0; 00226 ros::Rate rate(5); 00227 while (!initialized) 00228 { 00229 rate.sleep(); 00230 ros::spinOnce(); 00231 } 00232 for (int i = 7; i < 22; ++i) 00233 { 00234 if (zero) 00235 { 00236 r+= r_curr[i] - r_zero[i]; 00237 l+= l_curr[i] - l_zero[i]; 00238 } 00239 else 00240 { 00241 r+= r_curr[i]; 00242 l+= l_curr[i]; 00243 } 00244 } 00245 } 00246 00247 00248 00249 00250 void Pressure::getFront(double &r, double &l, bool zero) 00251 { 00252 r = 0; 00253 l = 0; 00254 ros::Rate rate(5); 00255 while (!initialized) 00256 { 00257 rate.sleep(); 00258 ros::spinOnce(); 00259 } 00260 for (int i = 3; i <= 4 ; ++i) 00261 { 00262 if (zero) 00263 { 00264 r+= r_curr[i] - r_zero[i]; 00265 l+= l_curr[i] - l_zero[i]; 00266 } 00267 else 00268 { 00269 r+= r_curr[i]; 00270 l+= l_curr[i]; 00271 } 00272 } 00273 } 00274 00275 00276 00277 void Pressure::getInsideTouched(double &r, double &l) 00278 { 00279 r = 0; 00280 l = 0; 00281 ros::Rate rate(5); 00282 while (!initialized) 00283 { 00284 rate.sleep(); 00285 ros::spinOnce(); 00286 } 00287 for (int i = 7; i < 22; ++i) 00288 { 00289 r+= touched[0][i]; 00290 l+= touched[1][i]; 00291 } 00292 } 00293 00294 void Pressure::getFrontTouched(double &r, double &l) 00295 { 00296 r = 0; 00297 l = 0; 00298 ros::Rate rate(5); 00299 while (!initialized) 00300 { 00301 rate.sleep(); 00302 ros::spinOnce(); 00303 } 00304 for (int i = 3; i < 4; ++i) 00305 { 00306 r+= touched[0][i]; 00307 l+= touched[1][i]; 00308 } 00309 }