/home/laradmin/lar/sensors/imu/imu_network/src/complementary_filter.cpp
Go to the documentation of this file.00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, 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 "ros/ros.h" 00035 #include "std_msgs/String.h" 00036 #include <sstream> 00037 #include <string> 00038 #include <iostream> 00039 #include <math.h> 00040 #include <cmath> 00041 #include <vector> 00042 00043 #include <boost/format.hpp> 00044 00045 #include <imu_network/sensors_network.h> 00046 #include <imu_network/filtered_imu_network.h> 00047 #include <imu_network/filtered_imu.h> 00048 00049 #define MAX_MEAN_IT 25 //number of iterations to measure the mean values from gyroscopes 00050 00051 #define GYRO_CONVERT M_PI/180*0.069565 // conversion of gyro's values from ADC to rad/sec 00052 00053 using namespace std; 00054 00055 00056 ros::NodeHandle* n; 00057 ros::Publisher chatter_pub; 00058 00059 int sensors_number; 00060 int first_run=0; 00061 00062 vector<float>gyro_angle[3]; 00063 vector<float>accel_ang_x; 00064 vector<float>accel_ang_y; 00065 00066 vector<float>angle[3]; 00067 vector<float>angle_last_it[3]; 00068 00069 vector<float>gyro_mean_vect[MAX_MEAN_IT][3]; //vectors for allocation of values of gyro for mean 00070 vector<float>gyro_real_mean[3]; //vetor of gyro's means 00071 vector<float>gyro_last_it[3]; 00072 00073 float period = 1/7.35; 00074 float alpha = 0.3; 00075 float accel_alpha = 0.7; 00076 00077 int mean_it = 0; 00078 00079 imu_network::filtered_imu_network complementary_filter_network; 00080 00081 void chatterCallback(const imu_network::sensors_network::ConstPtr& msg) //callback that intercept the sensor's raw data 00082 { 00083 float ax,ay,az; 00084 if(first_run ==0) 00085 { 00086 00087 sensors_number=(int)msg->sensors_val[0].total_number; 00088 00089 for (int i=0;i<sensors_number;i++) 00090 { 00091 //published message initialization 00092 imu_network::filtered_imu filtered_imu_data; 00093 complementary_filter_network.filtered_imu_network.push_back(filtered_imu_data); 00094 00095 ax = msg->sensors_val[i].S_Ax; 00096 ay = msg->sensors_val[i].S_Ay; 00097 az = msg->sensors_val[i].S_Az; 00098 00099 accel_ang_x.push_back(atan(ay/sqrt((ax*ax)+(az*az)))); 00100 accel_ang_y.push_back(atan(ax/sqrt((ay*ay)+(az*az)))); 00101 00102 gyro_angle[0].push_back(0); 00103 gyro_angle[1].push_back(0); 00104 gyro_angle[2].push_back(0); 00105 00106 gyro_last_it[0].push_back(0); 00107 gyro_last_it[1].push_back(0); 00108 gyro_last_it[2].push_back(0); 00109 00110 angle_last_it[0].push_back(accel_ang_x[i]); 00111 angle_last_it[1].push_back(accel_ang_y[i]); 00112 angle_last_it[2].push_back(0); 00113 00114 angle[0].push_back(0); 00115 angle[1].push_back(0); 00116 angle[2].push_back(0); 00117 00118 } 00119 00120 first_run = 1; 00121 return; 00122 } 00123 00124 //getting the gyro's means... 00125 if(mean_it<MAX_MEAN_IT) 00126 { 00127 for(int i= 0;i<sensors_number;i++) 00128 { 00129 gyro_mean_vect[i][0].push_back(msg->sensors_val[i].S_Gx); 00130 gyro_mean_vect[i][1].push_back(msg->sensors_val[i].S_Gy); 00131 gyro_mean_vect[i][2].push_back(msg->sensors_val[i].S_Gz); 00132 gyro_real_mean[0].push_back(i); 00133 gyro_real_mean[1].push_back(i); 00134 gyro_real_mean[2].push_back(i); 00135 } 00136 mean_it++; 00137 return; 00138 } 00139 //getting gyros's means to compensate it (if it's needed!) 00140 if(mean_it==MAX_MEAN_IT) 00141 { 00142 for(int i= 0;i<sensors_number;i++) 00143 { 00144 for(int j=0;j<mean_it;j++) 00145 { 00146 gyro_real_mean[0][i]+=gyro_mean_vect[i][0][j]; 00147 gyro_real_mean[1][i]+=gyro_mean_vect[i][1][j]; 00148 gyro_real_mean[2][i]+=gyro_mean_vect[i][2][j]; 00149 } 00150 gyro_real_mean[0][i]/=MAX_MEAN_IT; 00151 gyro_real_mean[1][i]/=MAX_MEAN_IT; 00152 gyro_real_mean[2][i]/=MAX_MEAN_IT; 00153 cout<<"mean_it = "<<mean_it<<" i="<<i<<" gyro_real_mean(0)="<<gyro_real_mean[0][i]<<" gyro_real_mean(1)="<<gyro_real_mean[1][i]<<" gyro_real_mean(2)="<<gyro_real_mean[2][i]<<endl; 00154 } 00155 mean_it++; 00156 } 00157 00158 float sensor_var[sensors_number][9]; 00159 00160 for(int i = 0; i<sensors_number; i++) 00161 { 00162 //sensors_values -> variable changing 00163 sensor_var[i][0]=msg->sensors_val[i].S_Ax; 00164 sensor_var[i][1]=msg->sensors_val[i].S_Ay; 00165 sensor_var[i][2]=msg->sensors_val[i].S_Az; 00166 sensor_var[i][3]=msg->sensors_val[i].S_Mx; 00167 sensor_var[i][4]=msg->sensors_val[i].S_My; 00168 sensor_var[i][5]=msg->sensors_val[i].S_Mz; 00169 sensor_var[i][6]=msg->sensors_val[i].S_Gx; 00170 sensor_var[i][7]=msg->sensors_val[i].S_Gy; 00171 sensor_var[i][8]=msg->sensors_val[i].S_Gz; 00172 00173 // getting Roll & Pitch from accelerometers 00174 ax = sensor_var[i][0]; 00175 ay = sensor_var[i][1]; 00176 az = sensor_var[i][2]; 00177 00178 accel_ang_x[i] = (accel_alpha * atan(ay/sqrt((ax*ax)+(az*az)))) + ((1-accel_alpha) * angle_last_it[0][i]); 00179 accel_ang_y[i] = (accel_alpha * atan(ax/sqrt((ay*ay)+(az*az)))) + ((1-accel_alpha) * angle_last_it[1][i]); 00180 00181 angle_last_it[0][i]=accel_ang_x[i]; 00182 angle_last_it[1][i]=accel_ang_y[i]; 00183 00184 // getting Roll , Pitch & Yaw from gyroscopes 00185 gyro_angle[0][i] = ((sensor_var[i][6]-gyro_real_mean[0][i])) * period * GYRO_CONVERT; 00186 gyro_angle[1][i] = ((sensor_var[i][7]-gyro_real_mean[1][i])) * period * GYRO_CONVERT; 00187 gyro_angle[2][i] = ((sensor_var[i][8]-gyro_real_mean[2][i])) * period * GYRO_CONVERT; 00188 00189 float accel_magnitude = sqrt(ax*ax + ay*ay + az*az); 00190 angle[0][i] = (1-alpha) * (angle[0][i] + gyro_angle[0][i]) + alpha * accel_ang_x[i]; 00191 angle[1][i] = (1-alpha) * (angle[1][i] + gyro_angle[1][i]) + alpha * accel_ang_y[i]; 00192 angle[2][i] = angle[2][i] + gyro_angle[2][i]; 00193 00194 // setting algorithm results on message variable 00195 complementary_filter_network.filtered_imu_network[i].linear_acceleration_x = sensor_var[i][0]; 00196 complementary_filter_network.filtered_imu_network[i].linear_acceleration_y = sensor_var[i][1]; 00197 complementary_filter_network.filtered_imu_network[i].linear_acceleration_z = sensor_var[i][2]; 00198 00199 complementary_filter_network.filtered_imu_network[i].angular_velocity_x = sensor_var[i][6]; 00200 complementary_filter_network.filtered_imu_network[i].angular_velocity_y = sensor_var[i][7]; 00201 complementary_filter_network.filtered_imu_network[i].angular_velocity_z = sensor_var[i][8]; 00202 00203 complementary_filter_network.filtered_imu_network[i].angular_displacement_x = angle[0][i]; 00204 complementary_filter_network.filtered_imu_network[i].angular_displacement_y = angle[1][i]; 00205 complementary_filter_network.filtered_imu_network[i].angular_displacement_z = angle[2][i]; 00206 00207 complementary_filter_network.filtered_imu_network[i].magnetometer_x = sensor_var[i][3]; 00208 complementary_filter_network.filtered_imu_network[i].magnetometer_y = sensor_var[i][4]; 00209 complementary_filter_network.filtered_imu_network[i].magnetometer_z = sensor_var[i][5]; 00210 00211 complementary_filter_network.filtered_imu_network[i].number = i; 00212 complementary_filter_network.filtered_imu_network[i].total_number= sensors_number; 00213 00214 complementary_filter_network.filtered_imu_network[i].algorithm = 1; 00215 00216 complementary_filter_network.filtered_imu_network[i].header.stamp = msg->sensors_val[i].header.stamp; 00217 complementary_filter_network.filtered_imu_network[i].header.frame_id = "filtered_imu_network"; 00218 00219 } 00220 chatter_pub.publish(complementary_filter_network); 00221 00222 } 00223 00228 int main(int argc, char **argv) 00229 { 00230 ros::init(argc, argv, "complementary_imu"); 00231 n = new(ros::NodeHandle); 00232 ros::Subscriber sub = n->subscribe("topic_raw_data", 1000, chatterCallback); 00233 chatter_pub = n->advertise<imu_network::filtered_imu_network>("topic_filtered_imu", 1000); 00234 00235 ros::spin(); 00236 00237 return 0; 00238 }