phantom_filter: /home/laradmin/lar/demos/phantom_filter/src/PhantomFilter.cpp Source File

00001   /**************************************************************************************************
00002   Software License Agreement (BSD License)
00003 
00004   Copyright (c) 2011-2014, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005   All rights reserved.
00006 
00007   Redistribution and use in source and binary forms, with or without modification, are permitted
00008   provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011   conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013   conditions and the following disclaimer in the documentation and/or other materials provided
00014   with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016   endorse or promote products derived from this software without specific prior written permission.
00017 
00018   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019   IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020   FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022   DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024   IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025   OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026   ***************************************************************************************************/
00027   
00034   #include "phantom_filter/PhantomFilter.h"
00035 
00036   const double PI = 3.141592653589793;
00037   using geometry_msgs::Point;
00038   using namespace std;
00039   using namespace ros;
00040 
00041   vector<Point> points,rots,forces; //points global
00042   Publisher marker_pub,text_pub,line_pub; // Rviz publisher
00043   Publisher phantom_pub; //phantom filter publisher 
00044   uint tam=200;//Buffer points size
00045   uint mod_func=0;// operation mode 
00046   int winSize=100;// lag for moving average (1 is no average at all)
00047 
00048 
00049   /******************Velocity Parameters(only Rviz)**********************/
00050   
00051   double inc=0,incma=0,incme=0,incmin=0,incul=0;
00052   double  fx1=0,fx2=0,fx3=0,fx4=0,fx5=0,fx6=0;
00053   double  fy1=0,fy2=0,fy3=0,fy4=0,fy5=0,fy6=0;
00054   double  fz1=0,fz2=0,fz3=0,fz4=0,fz5=0,fz6=0;
00055   Point l_v;
00056   Point l_std; 
00057 
00058   phantom_filter::Phantom g_State;
00059 
00060   /****************************Phantom State Callback**********************/
00061   
00062   void PhantomCallBk(const phantom_control::State &phantom_state){
00063   if(points.size()>tam)//Create vector points,rots,forces
00064   {
00065     points.erase(points.begin(),points.end()-1);
00066     rots.erase(rots.begin(),rots.end()-1);
00067     forces.erase(forces.begin(),forces.end()-1);
00068   }
00069 
00070   Point p,r,home,button,now_force;
00071   
00072   /*************Position**********/
00073   
00074   p.x = phantom_state.position[0];
00075   p.y = phantom_state.position[1];
00076   p.z = phantom_state.position[2];
00077 
00078 
00079   /************rot****************/
00080   
00081   r.x = phantom_state.rot[0]; 
00082   r.y = phantom_state.rot[1];
00083   r.z = phantom_state.rot[2];
00084 
00085 
00086   /************Home***************/
00087   
00088   home.x = phantom_state.home_pos[0];
00089   home.y = phantom_state.home_pos[1];
00090   home.z = phantom_state.home_pos[2];
00091 
00092 
00093   /***********Button***************/
00094   
00095   button.x=phantom_state.buttons[0];
00096   button.z=phantom_state.buttons[1];
00097 
00098 
00099   /************Force**************/
00100   
00101   now_force.x=phantom_state.force[0];
00102   now_force.y=phantom_state.force[1];
00103   now_force.z=phantom_state.force[2];
00104 
00105 
00106 
00107   points.push_back(p);
00108   rots.push_back(r);
00109   forces.push_back(now_force);
00110 
00111   if(points.size()==tam)
00112   {
00113     ProcessPoints(points); //Process Points
00114     ProcessRot(); //Process Rot
00115     //Velocity(points,home); // optional, without this all the program is based on position
00116     for(uint i=0;i<points.size();i++){
00117       if((points[i].x>home.x-3.0 && points[i].x<home.x+3.0) && (points[i].y>home.y-3.0 && points[i].y<home.y+3.0) && (points[i].z>home.z-3.0 && points[i].z<home.z+3.0))
00118       {
00119         points[i].x=home.x;points[i].y=home.y;points[i].z=home.z;  // if no function velocity use this to create a deadzone
00120       }
00121       
00122       /**********************************Phantom Filter points Publisher***************************************************************/
00123       
00124       if(isnan(points[i].x)) points[i].x=0;
00125       if(isnan(points[i].y)) points[i].y=0;
00126       if(isnan(points[i].z)) points[i].z=0;
00127       if(isnan(rots[i].x)) rots[i].x=0;
00128       if(isnan(rots[i].y)) rots[i].y=0;
00129       if(isnan(rots[i].z)) rots[i].z=0;
00130       
00131       g_State.Position[0]=points[i].x; g_State.Position[1]=points[i].y; g_State.Position[2]=points[i].z;
00132       g_State.Rot[0]=rots[i].x; g_State.Rot[1]=rots[i].y; g_State.Rot[2]=rots[i].z;
00133       g_State.Button[0]=button.x; g_State.Button[1]=button.z;   
00134       phantom_pub.publish(g_State);//Pub Position and orientation and button
00135     
00136         
00137       if(i%2==0) // Less Rate 
00138         Drawlines(points[i],rots[i],button,forces[i]);// Draw all points on Rviz
00139     }
00140   }
00141 
00142   }
00143 
00144   /***********************Velocity converter(Rviz only)*********************/
00145   
00146   void Velocity(vector<Point> v,Point h){
00147 
00148   Point std;
00149   std=Std(v); // Calculate standard deviation
00150 
00151   for(uint i=0;i<v.size();i++){
00152     if(abs(std.x)>30 ){
00153       if(v[i].x<5+h.x && v[i].x>-5+h.x){  // Home, velocity 0
00154         if(fx1==1){ inc=0; fx1=0; }
00155         l_v.x=l_v.x;
00156         points[i].x=l_v.x;
00157         fx4=1;fx3=1;fx2=1;fx5=1;fx6=0;
00158       }
00159       
00160       else if(v[i].x <h.x+25 && v[i].x>-25+h.x){ // home +/- 25 velocity 1
00161         if(fx2==1){ inc=0; fx2=0; }
00162         if(v[i].x<h.x+25 && v[i].x>h.x+5){
00163           points[i].x=l_v.x+incmin;
00164           l_v.x=points[i].x;
00165         }
00166         else if(v[i].x<-5+h.x && v[i].x>-25+h.x){ 
00167           points[i].x=l_v.x-incmin;
00168           l_v.x=points[i].x;
00169         }
00170         fx4=1;fx3=1;fx5=1;fx1=1,fx6=1;
00171       }
00172       
00173       else if(v[i].x <50+h.x && v[i].x>-50+h.x){// home +/- 50 velocity 2
00174         if(fx3==1){ inc=0; fx3=0; }
00175         if(v[i].x<50+h.x && v[i].x>25+h.x){
00176           points[i].x=l_v.x+incme;
00177           l_v.x=points[i].x;
00178         }
00179         else if(v[i].x<-25+h.x && v[i].x>-50+h.x){
00180           points[i].x=l_v.x-incme;
00181           l_v.x=points[i].x;
00182         }
00183         fx4=1;fx5=1;fx2=1;fx1=1,fx6=1;
00184       }
00185       
00186       else if(v[i].x <75+h.x && v[i].x>-75+h.x){// home +/- 75 velocity 3
00187         if(fx4==1){ inc=0; fx4=0; }
00188         if(v[i].x<75+h.x &&v[i].x>50+h.x){
00189           points[i].x=l_v.x+incul;
00190           l_v.x=points[i].x;
00191         }
00192         else if(v[i].x<-50+h.x &&v[i].x>-75+h.x){
00193           points[i].x=l_v.x-incul;
00194           l_v.x=points[i].x;
00195         }
00196         fx5=1;fx3=1;fx2=1;fx1=1,fx6=1;
00197       }
00198       
00199       else if(v[i].x >75+h.x || v[i].x<-75+h.x){ // home +/- 75+ velocity 4
00200         if(fx5==1){ inc=0; fx5=0; }
00201         if(v[i].x>75+h.x){
00202           points[i].x=l_v.x+incma;
00203           l_v.x=points[i].x;
00204           
00205         }
00206         else if(v[i].x<-75+h.x){
00207           points[i].x=l_v.x-incma;
00208           l_v.x=points[i].x;
00209         }
00210         fx4=1;fx3=1;fx2=1;fx1=1,fx6=1;
00211       }
00212       
00213     }else{points[i].x=l_v.x;}  
00214     
00215     // YYYY //
00216     if(abs(std.y)>30){ 
00217       if(v[i].y<5+h.y && v[i].y>-5+h.y){// Home, velocity 0
00218         if(fy1==1){ inc=0; fy1=0; }
00219         l_v.y=l_v.y;
00220         points[i].y=l_v.y;
00221         fy4=1;fy3=1;fy2=1;fy5=1,fy6=0;
00222       }
00223       
00224       
00225       else if(v[i].y <h.y+25 && v[i].y>-25+h.y){// home +/- 25 velocity 1
00226         if(fy2==1){ inc=0; fy2=0; }
00227         if(v[i].y<h.y+25 && v[i].y>h.y+5){
00228           points[i].y=l_v.y+incmin;
00229           l_v.y=points[i].y;
00230         }
00231         else if(v[i].y<-5+h.y && v[i].y>-25+h.y){
00232           points[i].y=l_v.y-incmin;
00233           l_v.y=points[i].y;
00234         }
00235         fy4=1;fy3=1;fy5=1;fy1=1,fy6=1;
00236       }
00237       
00238       else if(v[i].y <50+h.y && v[i].y>-50+h.y){// home +/- 50 velocity 2
00239         if(fy3==1){ inc=0; fy3=0; }
00240         if(v[i].y<50+h.y && v[i].y>25+h.y){
00241           points[i].y=l_v.y+incme;
00242           l_v.y=points[i].y;
00243         }
00244         else if(v[i].y>-50+h.y && v[i].y<-25+h.y){
00245           points[i].y=l_v.y-incme;
00246           l_v.y=points[i].y;
00247         }
00248         fy4=1;fy5=1;fy2=1;fy1=1,fy6=1;
00249       }
00250       
00251       else if(v[i].y <75+h.y && v[i].y>-75+h.y){// home +/- 75 velocity 3
00252         if(fy4==1){ inc=0; fy4=0; }
00253         if(v[i].y<75+h.y &&v[i].y>50+h.y){
00254           points[i].y=l_v.y+incul;
00255           l_v.y=points[i].y;
00256         }
00257         else if(v[i].y<-50+h.y &&v[i].y>-75+h.y){
00258           points[i].y=l_v.y-incul;
00259           l_v.y=points[i].y;
00260         }
00261         fy5=1;fy3=1;fy2=1;fy1=1,fy6=1;
00262       }
00263       
00264       else if(v[i].y >75+h.y || v[i].y<-75+h.y){// home +/- 75+ velocity 4
00265         if(fy5==1){ inc=0; fy5=0; }
00266         if(v[i].y>75+h.y){
00267           points[i].y=l_v.y+incma;
00268           l_v.y=points[i].y;  
00269         }
00270         else if(v[i].y<-75+h.y){
00271           points[i].y=l_v.y-incma;
00272           l_v.y=points[i].y;
00273         }
00274         fy4=1;fy3=1;fy2=1;fy1=1,fy6=1;
00275       }
00276       
00277     }else{points[i].y=l_v.y;}
00278     
00279     // zzzz //
00280     if(std.z>30){
00281       if(v[i].z<5+h.z && v[i].z>-5+h.z){//home,velocity 0
00282         if(fz1==1){ inc=0; fz1=0; }
00283         l_v.z=l_v.z;
00284         points[i].z=l_v.z;
00285         fz4=1;fz3=1;fz2=1;fz5=1,fz6=0;
00286       }
00287       
00288       else if(v[i].z <h.z+25 && v[i].z>-25+h.z){// home +/- 25 velocity 1
00289         if(fz2==1){ inc=0; fz2=0; }
00290         if(v[i].z<h.z+25 && v[i].z>h.z+5){
00291           points[i].z=l_v.z+incmin;
00292           l_v.z=points[i].z;
00293         }
00294         else if(v[i].z<-5+h.z && v[i].z>-25+h.z){
00295           points[i].z=l_v.z-incmin;
00296           l_v.z=points[i].z;
00297         }
00298         fz4=1;fz3=1;fz5=1;fz1=1,fz6=1;
00299       }
00300       
00301       else if(v[i].z <50+h.z && v[i].z>-50+h.z){// home +/- 50 velocity 2
00302         if(fz3==1){ inc=0; fz3=0; }
00303         if(v[i].z<50+h.z && v[i].z>25+h.z){
00304           points[i].z=l_v.z+incme;
00305           l_v.z=points[i].z;
00306         }
00307         else{
00308           points[i].z=l_v.z-incme;
00309           l_v.z=points[i].z;
00310         }
00311         fz4=1;fz5=1;fz2=1;fz1=1,fz6=1;
00312       }
00313       
00314       else if(v[i].z <75+h.z && v[i].z>-75+h.z){// home +/- 75 velocity 3
00315         if(fz4==1){ inc=0; fz4=0; }
00316         if(v[i].z<75+h.z &&v[i].z>50+h.z){
00317           points[i].z=l_v.z+incul;
00318           l_v.z=points[i].z;
00319         }
00320         else{
00321           points[i].z=l_v.z-incul;
00322           l_v.z=points[i].z;
00323         }
00324         fz5=1;fz3=1;fz2=1;fz1=1,fz6=1;
00325       }
00326       
00327       else if(v[i].z >75+h.z || v[i].z<-75+h.z){// home +/- 75+ velocity 4
00328         if(fz5==1){ inc=0; fz5=0; }
00329         if(v[i].z>75+h.z){
00330           points[i].z=l_v.z+incma;
00331           l_v.z=points[i].z;
00332           
00333         }
00334         else{
00335           points[i].z=l_v.z-incma;
00336           l_v.z=points[i].z;
00337         }
00338         fz4=1;fz3=1;fz2=1;fz1=1,fz6=1;
00339       }   
00340     }
00341     else{points[i].z=l_v.z;}
00342   }
00343 
00344   if(fz6==0) //Stop velocity inc
00345     inc=inc;
00346   else
00347     inc++;
00348 
00349   incma=inc/4; // velocity inc
00350   incmin=inc/7;
00351   incme=inc/6;
00352   incul=inc/5;
00353   }
00354   
00355 
00356   /****************Draw Rviz Line Points Text*****************/
00357   
00358   void Drawlines(Point v,Point r,Point b,Point f){ 
00359 
00360   /**********************Marker Cube******************************/
00361   
00362   visualization_msgs::Marker cube; // New Marker
00363   cube.header.frame_id = "/my_frame";//Frame
00364   cube.id = 0; 
00365   cube.header.stamp =Time::now();
00366   cube.ns = "phantom_rviz";
00367   cube.type =visualization_msgs::Marker::CUBE;
00368   cube.action= visualization_msgs::Marker::ADD;
00369   /*Position */
00370   if(mod_func==0)
00371   {
00372     cube.pose.position.x=v.x; 
00373     cube.pose.position.y=v.y;
00374     cube.pose.position.z=0;
00375     cube.pose.orientation.w = 1.0; 
00376     cube.pose.orientation.x=0;
00377     cube.pose.orientation.y=0;
00378     cube.pose.orientation.z=0;
00379   }
00380   else if(mod_func==1)
00381   {
00382     if(b.x==0)
00383     {
00384       cube.pose.position.x=0; 
00385       cube.pose.position.y=0;
00386       cube.pose.position.z=0;
00387       cube.pose.orientation.w = 1.0; 
00388       cube.pose.orientation.x=r.x;
00389       cube.pose.orientation.y=r.y;
00390       cube.pose.orientation.z=r.z;
00391     }
00392     else
00393     {
00394       cube.pose.position.x=0; 
00395       cube.pose.position.y=0;
00396       cube.pose.position.z=0;
00397       cube.pose.orientation.w = 1.0; 
00398       cube.pose.orientation.x=0;
00399       cube.pose.orientation.y=0;
00400       cube.pose.orientation.z=0;
00401     }
00402     
00403   }
00404   else{
00405     if(b.x==1)
00406     {
00407       cube.pose.position.x=v.x; 
00408       cube.pose.position.y=v.z;
00409       cube.pose.position.z=v.y;
00410       cube.pose.orientation.w = 1.0; 
00411       cube.pose.orientation.x=0;
00412       cube.pose.orientation.y=0;
00413       cube.pose.orientation.z=0;        
00414     }
00415     else
00416     {
00417       cube.pose.position.x=v.x; 
00418       cube.pose.position.y=v.z;
00419       cube.pose.position.z=v.y;
00420       cube.pose.orientation.w = 1.0; 
00421       cube.pose.orientation.x=r.x;
00422       cube.pose.orientation.y=r.y;
00423       cube.pose.orientation.z=r.z;      
00424     }
00425   }
00426 
00427   //Scale and color
00428   cube.scale.x = 20;
00429   cube.scale.y = 20;
00430   cube.scale.z = 20;
00431   cube.color.g = 1.0f;
00432   cube.color.a = 1.0;
00433 
00434 
00435   /***************Marker text***********************/
00436   
00437   visualization_msgs::Marker tex; // New Marker
00438   tex.header.frame_id = "/my_frame";//Frame
00439   tex.id = 100; 
00440   tex.header.stamp =Time::now();
00441   tex.ns = "phantom_rviz_tex";
00442   tex.type =visualization_msgs::Marker::TEXT_VIEW_FACING;
00443   tex.action= visualization_msgs::Marker::ADD;
00444   string ex=to_string((long double)f.x) +"N" ; 
00445   string ey=to_string((long double)f.y) +"N" ;
00446   string ez=to_string((long double)f.z) +"N" ;
00447   tex.text="" + ex+ '\n' +"" +ey +'\n' + ""+ez+'\n' ;
00448   tex.pose.position.x=600; 
00449   tex.pose.position.y=220;
00450   tex.pose.position.z=0;
00451 
00452   //Scale andColor */
00453   if(!Limits(v))
00454   {
00455     tex.scale.z = 'a';
00456     tex.color.r = 1.0f;
00457     tex.color.a = 1.0;
00458   }
00459   else
00460   {
00461     tex.scale.z = 'a';
00462     tex.color.g = 1.0f;
00463     tex.color.a = 1.0;
00464   }
00465 
00466   /************************Maker line**********************/
00467   
00468   visualization_msgs::Marker line_strip; // New Marker
00469   line_strip.header.frame_id = "/my_frame";//Frame
00470   line_strip.id = 200; 
00471   line_strip.header.stamp =Time::now();
00472   line_strip.ns = "phantom_rviz_line";
00473   line_strip.type =visualization_msgs::Marker::LINE_STRIP;
00474   line_strip.action= visualization_msgs::Marker::ADD;
00475 
00476   tex.pose.position.x=100; 
00477   tex.pose.position.y=220;
00478   tex.pose.position.z=0;
00479 
00480   //Scale and Color
00481   line_strip.scale.x = 5;
00482   line_strip.color.b = 1.0f;
00483   line_strip.color.a = 1.0;
00484 
00485 
00486   Point test ; test.x=-100 ;test.y=-100;
00487   line_strip.points.push_back(test);
00488   test.x=100;test.y=100;
00489   line_strip.points.push_back(test);
00490 
00491   //Publish both markers
00492   marker_pub.publish(cube);
00493   text_pub.publish(tex);
00494   line_pub.publish(line_strip);
00495   }
00496 
00497   /**************Limits Device***************/
00498   
00499   bool Limits(Point temp){
00500   if(temp.x>80 || temp.x<-80 || temp.y>60 || temp.y<-60 || temp.z>35 || temp.z<-35)
00501     return false;
00502   else 
00503     return true;
00504   }
00505 
00506   /*****************Standard deviation**************/
00507   
00508   Point Std(const vector<Point>& v){
00509 
00510   double s=0;
00511   vector<double> vx,vy,vz;
00512   Point p;
00513   p.x = 0;
00514   p.y = 0;
00515   p.z = 0;
00516 
00517   for(uint i=0;i<v.size();i++) // dead-zone
00518   {
00519     if(v[i].x>-3 && v[i].x<3)
00520     {
00521       vx.push_back(0);
00522     }else
00523       vx.push_back(v[i].x);
00524     
00525     if(v[i].y>-3 && v[i].y<3)
00526     {
00527       vy.push_back(0);
00528     }else
00529       vy.push_back(v[i].y);
00530     
00531     if(v[i].z>-3 && v[i].z<3)
00532     {
00533       vz.push_back(0);
00534     }else
00535       vz.push_back(v[i].z);    
00536     
00537   }
00538   double sumx=accumulate(vx.begin(),vx.end(),0.0);
00539   double sumy=accumulate(vy.begin(),vy.end(),0.0);
00540   double sumz=accumulate(vz.begin(),vz.end(),0.0);
00541   double meanx=sumx/vx.size();
00542   double meany=sumy/vy.size();
00543   double meanz=sumz/vz.size();
00544 
00545 
00546   double sq_sumx =inner_product(vx.begin(),vx.end(),vx.begin(),0.0);
00547   double sq_sumy =inner_product(vy.begin(),vy.end(),vy.begin(),0.0);
00548   double sq_sumz =inner_product(vz.begin(),vz.end(),vz.begin(),0.0);
00549   p.x=sqrt(sq_sumx/vx.size() -meanx*meanx);
00550   p.y=sqrt(sq_sumy/vy.size() -meany*meany);
00551   p.z=sqrt(sq_sumz/vz.size() -meanz*meanz);
00552 
00553   s=p.x+p.y+p.z;
00554   if(s!=0){ // standard deviation in %
00555     p.x=(p.x/s)*100; p.y=(p.y/s)*100; p.z=(p.z/s)*100;
00556     l_std.x=p.x;l_std.y=p.y;l_std.z=p.z;
00557   }
00558   else{
00559     if(mod_func==0){
00560       p.x=l_std.x; p.y=l_std.y; p.z=l_std.z; //velocity active
00561     }
00562     else if(mod_func==1){ //rots active
00563         p.x=0; p.y=0; p.z=0;
00564     }
00565     else{
00566         p.x=l_std.x; p.y=l_std.y; p.z=l_std.z;
00567     }
00568   }
00569   return p;
00570   }
00571 
00572   /*************Process Rotation****************/
00573   
00574   void ProcessRot(){
00575 
00576   Point p_std=Std(rots);
00577   Point p_mean_rot=Mean(rots);
00578 
00579   for(uint i=0;i<rots.size();i++)
00580   {
00581     if(p_std.x>p_std.y && p_std.x>p_std.z)
00582     {  
00583       rots[i].y=p_mean_rot.y;
00584       rots[i].z=p_mean_rot.z;   
00585     }
00586     else if(p_std.y>p_std.x && p_std.y>p_std.z)
00587     {
00588       rots[i].x=p_mean_rot.x;
00589       rots[i].z=p_mean_rot.z;
00590     }
00591     else if(p_std.z>p_std.y && p_std.z>p_std.x)
00592     {
00593       rots[i].x=p_mean_rot.x;
00594       rots[i].y=p_mean_rot.y;
00595     }
00596     
00597     rots[i].x=rots[i].x+PI; rots[i].y=rots[i].y+PI; rots[i].z=rots[i].z+PI; // Plus offset
00598     if(rots[i].x<0.15 && rots[i].x>-0.15)
00599       rots[i].x=0.0;
00600     if(rots[i].y<0.15 && rots[i].y>-0.15)
00601       rots[i].y=0.0;
00602     if(rots[i].z<0.15 && rots[i].z>-0.15)
00603       rots[i].z=0.0;
00604   }    
00605   rots=Moving_Average(rots,winSize);    
00606   }
00607 
00608   /***********************Process Points*********************/
00609   
00610   void ProcessPoints(vector<Point> V){
00611 
00612   double res=PI/2;//the allowed step for directions; resolution of slopes
00613   double bThresh=0;// threshold for data variation dead-zone
00614   vector<Point> newpoint,newdiff,newdom,newsqrt;
00615 
00616   newpoint=Moving_Average(V,winSize);  // Calculate moving average of data and their derivatives
00617 
00618   newdiff=Diff(newpoint);
00619 
00620   newpoint=Moving_Average(newdiff,winSize);
00621 
00622   newpoint=BThresh(newpoint,bThresh);  // adjust dead-zones in derivatives (minimum allowed displacements)
00623   
00624   newdom=Atan2(newpoint); // Calculate direction of motion 
00625 
00626   newdom=Round(newdom,res); // ... and the relative contributions in fractions of slope resolutions
00627 
00628   newsqrt=Sqrt(newpoint); //Add the starting point for proper integration
00629 
00630   newpoint=Insert(points,newsqrt,newdom,res);
00631 
00632   points=CumSum(newpoint); // restore X, Y data 
00633   }
00634 
00635   /*******************************Insert*********************************************************/
00636   
00637   vector<Point> Insert(vector<Point> v_old,vector<Point> v_new,vector<Point> dom,double r){  
00638   Point p; p.x=0; p.y=0; p.z=0;
00639   Point n; n.x=0; n.y=0; n.z=0;
00640   p.x=v_old[0].x; p.y=v_old[0].y; p.z=v_old[0].z;
00641   vector<Point> temp;
00642   temp.push_back(p);
00643 
00644   for(uint i=0;i<v_new.size();i++)
00645   {
00646     n.x=v_new[i].x*cos(r*dom[i].x);
00647     n.y=v_new[i].x*sin(r*dom[i].x);
00648     n.z=v_new[i].z*cos(r*dom[i].z);
00649     temp.push_back(n);
00650   }
00651   return temp;
00652   }
00653 
00654   /*************************Bthresh*****************************/
00655   
00656   vector<Point> BThresh(vector<Point> V,double bt){  
00657   vector<Point> temp;
00658   Point p; p.x=0; p.y=0; p.z=0;
00659 
00660   for(uint i=0;i<V.size();i++)
00661   {
00662     if(abs(V[i].x)<bt) p.x=0;
00663     else p.x=V[i].x;
00664     if(abs(V[i].y)<bt) p.y=0;
00665     else p.y=V[i].y;
00666     if(abs(V[i].z)<bt) p.z=0;
00667     else p.z=V[i].z;
00668     temp.push_back(p);
00669   }
00670   return temp;
00671   }
00672 
00673   /************************Diff*********************************/
00674   
00675   vector<Point> Diff(vector<Point> V){
00676   vector<Point> temp;
00677   Point p; p.x=0; p.y=0; p.z=0;
00678   //Sum [i] - [i-1]
00679   for(uint i=1;i<V.size();i++)
00680   {
00681     p.x=V[i].x-V[i-1].x;
00682     p.y=V[i].y-V[i-1].y;
00683     p.z=V[i].z-V[i-1].z;
00684     temp.push_back(p); 
00685   }
00686   return temp;
00687   }
00688 
00689   /************************Arctangent****************************/
00690   
00691   vector<Point> Atan2(vector<Point> V){
00692   vector<Point> temp;
00693   Point p; p.x=0; p.y=0; p.z=0;
00694 
00695   for(uint i=0;i<V.size();i++){
00696     p.x=atan2(V[i].y,V[i].x);//have the information btw x and y
00697     p.z=atan2(V[i].y,V[i].z);//have the information btw z and y
00698     p.y=0;
00699     temp.push_back(p);
00700   }
00701   return temp;
00702   }
00703 
00704   /**************************SQRT*******************************/
00705   
00706   vector<Point> Sqrt(vector<Point> V){
00707 
00708   vector<Point> temp;
00709   Point p; p.x=0; p.y=0; p.z=0;
00710 
00711   for(uint i=0;i<V.size();i++)
00712   {
00713     p.x=sqrt(V[i].x*V[i].x + V[i].y*V[i].y);
00714     p.z=sqrt(V[i].z*V[i].z + V[i].y*V[i].y);
00715     p.y=0;
00716     temp.push_back(p);
00717   }
00718   return temp;
00719   }
00720 
00721   /************************CumSum******************************/
00722   
00723   vector<Point> CumSum(vector<Point> V){
00724   vector<Point> temp;
00725   Point p; p.x=0; p.y=0; p.z=0;
00726   p.x=V[0].x; p.y=V[0].y; p.z=V[0].z;
00727   temp.push_back(p);
00728   //Sum [i] + [i+1]
00729   for(uint i=1;i<V.size();i++)
00730   {
00731     p.x=temp[i-1].x + V[i].x;
00732     p.y=temp[i-1].y + V[i].y;
00733     p.z=temp[i-1].z + V[i].z;
00734     temp.push_back(p);
00735   }
00736   return temp;
00737   }
00738 
00739   /********************Round**********************************/
00740   
00741   vector<Point> Round(vector<Point> V, double res){
00742   vector<Point> temp;
00743   Point p; p.x=0; p.y=0; p.z=0;
00744 
00745   //roud point with a slope of res
00746   for( uint i=0; i<V.size();i++)
00747   {
00748     p.x=round(V[i].x/res);
00749     p.y=round(V[i].x/res);
00750     p.z=round(V[i].z/res);
00751     temp.push_back(p);
00752   }
00753   return temp;
00754   }
00755 
00756   /**********************Moving mean Filter********************/
00757   
00758   vector<Point> Moving_Average(vector<Point> V,int winSize){
00759   vector<Point> temp,temp2;
00760   Point p;
00761   p.x=0; p.y=0; p.z=0;
00762   uint Max=0;
00763   int size=V.size();
00764   for(int i=0;i<size;i++)
00765   {
00766     Max=max(0,i-winSize);
00767     temp2=Copy(V,Max,i);
00768     p=Mean(temp2);
00769     temp.push_back(p);
00770   }
00771   return temp;
00772   }
00773 
00774   /************************Copy*********************************/
00775   
00776   vector<Point> Copy(vector<Point> V,uint m,int n){ 
00777   vector<Point> temp;
00778   Point p; p.x=0; p.y=0; p.z=0;
00779   //copy every point btw m and n
00780   if(m==0 && n==0)
00781   {
00782     p.x=V[0].x;
00783     p.y=V[0].y;
00784     p.z=V[0].z;
00785     temp.push_back(p);
00786   }
00787   else
00788   {
00789     for(int i=m;i<n;i++) 
00790     {
00791       p.x=V[i].x;
00792       p.y=V[i].y;
00793       p.z=V[i].z;
00794       temp.push_back(p);
00795     }
00796   }
00797   return temp;
00798   }
00799 
00800   /*********************Mean Function***************************/
00801   
00802   Point Mean(const vector<Point>& v){ 
00803   vector<double> vx,vy,vz; // vector Position x , y and z
00804   Point p;
00805   p.x = 0;
00806   p.y = 0;
00807   p.z = 0;
00808 
00809   for(uint i=0;i<v.size();i++)//Separate vector v into 3 different vectors
00810   {
00811     vx.push_back(v[i].x);
00812     vy.push_back(v[i].y);
00813     vz.push_back(v[i].z);
00814   }
00815   double sumx=accumulate(vx.begin(),vx.end(),0.0);
00816   double sumy=accumulate(vy.begin(),vy.end(),0.0);
00817   double sumz=accumulate(vz.begin(),vz.end(),0.0);
00818   p.x=sumx/vx.size();
00819   p.y=sumy/vy.size();
00820   p.z=sumz/vz.size();
00821   return p;
00822   }
00823 
00829   int main( int argc,char** argv){
00830     init(argc,argv,"phant_filter_node");
00831     cout<<"Operation mode: "; //Operation mode:
00832     cin>>mod_func;
00833     NodeHandle n;
00834     
00835     /********************************Rviz publishers*************************************/
00836     
00837     marker_pub = n.advertise<visualization_msgs::Marker>("my_marker_points", 100);//Points
00838     text_pub=n.advertise<visualization_msgs::Marker>("my_text_points",100); //Text Force
00839     line_pub=n.advertise<visualization_msgs::Marker>("my_line_points",100);//Line
00840     
00841     /**********************************Phantom****************************************************/
00842     
00843     phantom_pub = n.advertise<phantom_filter::Phantom>("phantom_filter", 100); // points filter publisher
00844     Subscriber sub__force = n.subscribe ( "/phantom_state", 100, PhantomCallBk );//Phantom control callback
00845     
00846     spin();
00847     return 0;
00848   }