acceleration_observer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Joe Romano
00036  *********************************************************************/
00037 //@author  Joe Romano
00038 //@email   joeromano@gmail.com
00039 //@brief   acceleration_observer.cpp - file to read the hand mounted 
00040 //         accelerometer in the pr2 and do some simple processing to 
00041 //         make the data more accessible to higher-level code
00042 
00043 #include <pr2_gripper_sensor_controller/acceleration_observer.h>
00044 
00045 // method to be called to check if we saw  a contact acceleration
00046 bool accelerationObserver::checkPlaceContact(double dAcc)
00047 {
00048   //if(( fabs(aX) > dAcc) || (fabs(aY) > dAcc)  || (fabs(aZ) > dAcc)|| placeContact )
00049   if( sqrt(aX_bp*aX_bp + aY_bp*aY_bp + aZ_bp*aZ_bp) > dAcc || placeContact)
00050     placeContact = true;
00051   else
00052     placeContact = false;
00053   return placeContact;
00054 }
00055 
00056 // method to be called each 1ms timestep in realtime to ensure data collection and manipulation happens properly
00057 void accelerationObserver::spin()
00058 {
00059   // retrieve and filter our acceleration data
00060   std::vector<geometry_msgs::Vector3> threeAccs = accHandle->state_.samples_;
00061   for( uint  i = 0; i < threeAccs.size(); i++ )  
00062   {
00063     aX_bp = accBPFilt[0]->getNextFilteredValue(threeAccs[i].x);
00064     aY_bp = accBPFilt[1]->getNextFilteredValue(threeAccs[i].y);
00065     aZ_bp = accBPFilt[2]->getNextFilteredValue(threeAccs[i].z);
00066 
00067     aX_lp = accLPFilt[0]->getNextFilteredValue(threeAccs[i].x);
00068     aY_lp = accLPFilt[1]->getNextFilteredValue(threeAccs[i].y);
00069     aZ_lp = accLPFilt[2]->getNextFilteredValue(threeAccs[i].z);
00070     readingTime = ros::Time::now().toSec();
00071   }    
00072 
00073 }
00074 
00075 // constructor
00076 accelerationObserver::accelerationObserver(pr2_hardware_interface::Accelerometer* accelerometerHandle)
00077 {
00078   accHandle = accelerometerHandle;
00079 
00080   // place contact flag
00081   placeContact = false;
00082 
00083   // instatiate our acceleration values
00084   aX_lp = 0;
00085   aY_lp = 0;
00086   aZ_lp = 0;
00087   aX_bp = 0;
00088   aY_bp = 0;
00089   aZ_bp = 0;
00090 
00091   // make sure the accelerometer has a 1.5 khz bandwidth          
00092   accHandle->command_.bandwidth_ = 6;
00093 
00094   // make sure the accelerometer has a +/- 8g range (0 = 2g, 1 =  4g)         
00095   accHandle->command_.range_ = 2;
00096 
00097   
00098   // create our filter for band-passed accelerometer data
00099   // 1st order chebychev. band-pass 5-1000 hz
00100   float b_bpfilt[] = {0.8305, 0, -0.8305};
00101   float a_bpfilt[] = {1.0,-0.3329,-0.6610};
00102   for(int i=0; i < 3; i++)
00103     accBPFilt[i] = new digitalFilter(2, true,b_bpfilt,a_bpfilt);
00104 
00105   // create our filter for low-passed accelerometer data
00106   // 1st order butterworth. low-pass 1000 hz
00107   float b_lpfilt[] = {0.634, 0.634};
00108   float a_lpfilt[] = {1.0, 0.2679};
00109   for(int i=0; i < 3; i++)
00110     accLPFilt[i] = new digitalFilter(1, true,b_lpfilt,a_lpfilt);
00111   
00112 }
00113 
00114 
00115 // destructor
00116 accelerationObserver::~accelerationObserver()
00117 {
00118   delete[] accBPFilt;
00119   delete[] accLPFilt;
00120 }
00121 


pr2_gripper_sensor_controller
Author(s): Joe Romano
autogenerated on Mon Oct 6 2014 12:20:21