mtig_driver: sensorData.cpp Source File

Go to the documentation of this file.
00001 
00011 #include <sensorData.h>
00012 
00016 SensorData::SensorData( outputSettings  & mSettings){
00017   
00018         accX = accY = accZ = 0.0;
00019         gyrX = gyrY = gyrZ = 0.0;
00020         magX = magY = magZ = 0.0;
00021         q0 = q1 = q2 = q3 = 0.0;
00022         eroll = epitch = eyaw = 0.0;
00023         mTemperature = mPressure = 0.0;
00024         mVelocityX = mVelocityY = mVelocityZ = 0.0;
00025         gpsVelocityX = gpsVelocityY = gpsVelocityZ = 0.0;
00026         mStatus = 0;
00027         mHorizontalAccuracy = mVerticalAccuracy = 0;
00028         mAltitude = mLongitude = mLatitude = 0.0;
00029         mVelocityNorth = mVelocityEast = mVelocityDown = 0.0;
00030         
00031         
00032 
00033         //Get measurements errors from the launchfile. They should be already ^2 because they'll go straight to the covariance matrix
00034         if(ros::param::get("~roll_error", roll_error)){
00035                 roll_error=to_rad_sqr(roll_error);              
00036         }
00037         else{
00038                 ROS_ERROR("No roll_error value defined in launchfile"); 
00039         }
00040 
00041         if(ros::param::get("~pitch_error", pitch_error)){
00042                 pitch_error=to_rad_sqr(pitch_error);            
00043         }
00044         else{
00045                 ROS_ERROR("No pitch_error value defined in launchfile");        
00046         }
00047 
00048         if(ros::param::get("~yaw_error", yaw_error)){
00049                 yaw_error=to_rad_sqr(yaw_error);                
00050         }
00051         else{
00052                 ROS_ERROR("No yaw_error value defined in launchfile");  
00053         }
00054 
00055         if(!ros::param::get("~acc_noise", acc_noise)){
00056                 ROS_ERROR("No acc_error value defined in launchfile");  
00057         }
00058         else{
00059                 acc_error=calculateAccErrorSquared(acc_noise, mSettings.accelerationFreq);
00060         }       
00061 
00062         if(!ros::param::get("~gyr_noise", gyr_noise)){
00063                 ROS_ERROR("No noise_density value defined in launchfile");
00064         }
00065         else{
00066                 gyr_error= calculateGyrErrorSquared(gyr_noise, mSettings.gyroscopeFreq);
00067         }
00068 
00069 
00070         ROS_DEBUG_STREAM("GYR_ERROR=" << gyr_error);
00071         ros::param::param<std::string>("~frame_id", frameId_string, "xsens");
00072 }
00073 SensorData::SensorData(){
00074 
00075 }
00076 
00077 
00084 float SensorData::calculateAccErrorSquared(float noise_density, float freq){
00085         return (noise_density*GRAVITY)*(noise_density*GRAVITY)*freq;
00086 
00087 }
00088 
00089 
00096 float SensorData::calculateGyrErrorSquared(float noise_density, float freq){
00097         return to_rad_sqr(noise_density)*freq;
00098 }
00099 
00104 void SensorData::fillData(XsDataPacket * _packet){
00105                 XsDataPacket packet = *_packet;
00106                 XsMessage msg = packet.toMessage();
00107                 XsSize msg_size = msg.getDataSize();
00108 
00109                 ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Data Size: " << msg_size);
00110 
00111                 
00112         // Get the orientation data
00113                 if (packet.containsOrientation()) {
00114                         XsQuaternion quaternion = packet.orientationQuaternion();
00115 
00116                         q1 = quaternion.x();
00117                         q2 = quaternion.y();
00118                         q3 = quaternion.z();
00119                         q0 = quaternion.w();
00120                                                 
00121                         XsEuler euler = packet.orientationEuler();
00122                         eroll = euler.roll();
00123                         epitch = euler.pitch();
00124                         eyaw = euler.yaw();
00125                         
00126                         // ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,"Orientation: Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll
00127                         //                                << ", Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch
00128                         //                                << ", Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw);
00129                                                 
00130                 }
00131 
00132                 // Get the gyroscope data
00133                 if (packet.containsCalibratedGyroscopeData()) {
00134                         XsVector gyroscope = packet.calibratedGyroscopeData();
00135                 
00136                         gyrX = gyroscope.at(0);
00137                         gyrY = gyroscope.at(1);
00138                         gyrZ = gyroscope.at(2);
00139 
00140                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Angular Velocity: ( "
00141                                                          << gyrX << ", " << gyrY << ", " << gyrZ << ")" ); 
00142                 }
00143 
00144                 // Get the acceleration data
00145                 if (packet.containsCalibratedAcceleration()) {
00146                         XsVector acceleration = packet.calibratedAcceleration();
00147 
00148                         accX = acceleration.at(0);
00149                         accY = acceleration.at(1);
00150                         accZ = acceleration.at(2);
00151                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Linear Acceleration: (" 
00152                                                         << accX << "," << accX << "," << accZ << ")" );
00153                 }
00154 
00155                 //Get the altitude data
00156                 if(packet.containsAltitude()){
00157                         mAltitude = packet.altitude();
00158                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsAltitude: ");
00159                 }
00160 
00161                         
00162                 //Get the Latitude and Longitude Data
00163                 if(packet.containsLatitudeLongitude()){
00164                         XsVector latlon = packet.latitudeLongitude();
00165                         mLatitude = latlon[0];
00166                         mLongitude = latlon[1];
00167                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsLatitudeLongitude");
00168                 }
00169 
00170                 // Get the magnetic field data
00171                 if (packet.containsCalibratedMagneticField()) {
00172                         XsVector magneticField = packet.calibratedMagneticField();
00173 
00174                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Magnetic Field: ("<< magneticField[0] <<" , " 
00175                                                         << magneticField[1] << " , "<< magneticField[2]   << ")");      
00176 
00177                         magX = magneticField.at(0);
00178                         magY = magneticField.at(1);
00179                         magZ = magneticField.at(2);
00180                 }
00181                                         
00182                 // Get Temperature data
00183                 if (packet.containsTemperature()){
00184                         double temperature = packet.temperature();
00185                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Temperature : " << temperature);
00186                         mTemperature = temperature;
00187                 }
00188 
00189                 // Get Pressure Data 
00190                 if(packet.containsPressure() ){
00191                         XsPressure pressure = packet.pressure();
00192                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pressure : " << pressure.m_pressure);
00193                         mPressure= pressure.m_pressure;
00194                 }       
00195         
00196                 // Get Velocity Data
00197                 if( packet.containsVelocity() ){
00198                         XsVector velocity = packet.velocity();
00199                         mVelocityX = velocity.at(0);
00200                         mVelocityY = velocity.at(1);
00201                         mVelocityZ = velocity.at(2);
00202 
00203                         ROS_INFO_STREAM(" Velocity [ x (east) : " << mVelocityX << ", y (north) : "
00204                                         << mVelocityY << ", z (up) " << mVelocityZ << " ]");
00205                 }
00206 
00207 
00208                 // Get GPS PVT Data
00209                 if( packet.containsGpsPvtData() ){
00210                         XsGpsPvtData gpsPvtData = packet.gpsPvtData();
00211                         ROS_INFO_STREAM(" Horizontal accuracy: " << gpsPvtData.m_hacc ); 
00212                 }
00213 
00214                 if(packet.containsRawAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Acceleration") ;}
00215                 if(packet.containsRawGyroscopeData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Gyroscope") ;}
00216                 if(packet.containsRawGyroscopeTemperatureData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawGyrTemp");}
00217                 if(packet.containsRawMagneticField() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawMag");}
00218                 if(packet.containsRawTemperature() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawTemp");}
00219                 if(packet.containsRawData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawData");}
00220                 if(packet.containsCalibratedData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Calib Data");}
00221                 if(packet.containsSdiData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SDI");}
00222                 if(packet.containsStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "StatusByte");}
00223                 if(packet.containsDetailedStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "DetailedStatus");}
00224                 if(packet.containsPacketCounter8() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter8");}
00225                 if(packet.containsPacketCounter() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter");}
00226                 if(packet.containsSampleTimeFine() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeFine");}
00227                 if(packet.containsSampleTimeCoarse() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeCoarse");}
00228                 if(packet.containsSampleTime64() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTime64");}
00229                 if(packet.containsFreeAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "FreeAcceleration");}
00230                 if(packet.containsPressureAge() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PressureAge");}
00231                 if(packet.containsAnalogIn1Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN1IN");}
00232                 if(packet.containsAnalogIn2Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN2IN");}
00233                 if(packet.containsPositionLLA() ){ 
00234                         
00235                         XsVector posLLA = packet.positionLLA();
00236                         mLatitude = posLLA[0];
00237                         mLongitude = posLLA[1];
00238                         mAltitude = posLLA[2];
00239                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLatitude = " << mLatitude);
00240                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLongitude = " << mLongitude);
00241                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mAltitude = " << mAltitude);
00242 
00243                         //for(int i=0; i < posLLA.size(); i++){
00244                         //ROS_INFO_STREAM("posLLA[" << i << "] : " << posLLA[i] ); 
00245                         //}
00246                 }
00247                 if(packet.containsUtcTime() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "UTC-Time");}
00248                 if(packet.containsFrameRange() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Frame Range");}
00249                 if(packet.containsRssi() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RSSI");}
00250                 if(packet.containsRawGpsDop() ){ 
00251                         //ROS_INFO_THROTTLE(THROTTLE_VALUE, "DOP");}
00252                         XsRawGpsDop dop = packet.rawGpsDop();
00253                         m_gdop = ((float) dop.m_gdop)/100;
00254                         m_pdop = ((float) dop.m_pdop)/100;
00255                         m_tdop = ((float) dop.m_tdop)/100;
00256                         m_vdop = ((float) dop.m_vdop)/100;
00257                         m_hdop = ((float) dop.m_hdop)/100;
00258                         m_edop = ((float) dop.m_edop)/100;
00259                         m_ndop = ((float) dop.m_ndop)/100;
00260                         m_itow = ((float) dop.m_itow)/1000;
00261 
00262                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, 
00263                                                 "GDOP = " << m_gdop <<
00264                                                 ",PDOP = " << m_pdop <<                                         
00265                                                 ",TDOP = " << m_tdop <<                                         
00266                                                 ",VDOP = " << m_vdop <<                                         
00267                                                 ",HDOP = " << m_hdop <<                                         
00268                                                 ",EDOP = " << m_edop <<                                         
00269                                                 ",NDOP = " << m_ndop <<                                         
00270                                                 ",ITOW = " << m_itow);                  
00271                 }
00272                 if(packet.containsRawGpsSol() ){
00273                         // ROS_INFO_THROTTLE(THROTTLE_VALUE, "SOL");}
00274                         XsRawGpsSol sol = packet.rawGpsSol();
00275                         mPositionAccuracy = sol.m_pacc;
00276                         mSpeedAccuracy = sol.m_sacc;
00277                         mSatelliteNumber = sol.m_numsv;
00278                         mGpsFixStatus = sol.m_gpsfix;
00279                         
00280                         gpsVelocityX=sol.m_ecef_vx/100;
00281                         gpsVelocityY=sol.m_ecef_vy/100;
00282                         gpsVelocityZ=sol.m_ecef_vz/100;
00283                         ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,
00284                                                 "Pos Acc = " << mPositionAccuracy <<
00285                                                 ",Speed Acc = " << mSpeedAccuracy <<
00286                                                 ",Sat Number = " << mSatelliteNumber <<
00287                                                 ",GPS FIX = " << std::hex << mGpsFixStatus);
00288                 }
00289                 if(packet.containsRawGpsTimeUtc() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "GPS-UTC");}
00290                 if(packet.containsRawGpsSvInfo() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SV INFO");}
00291                 //      if(packet.containsTriggerIndication() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "TRIGGER");}
00292 
00293 
00294 
00295 
00296                 //Get Status byte               
00297                 if( packet.containsStatus() ){
00298                         mStatus = packet.status();
00299                         ROS_INFO_THROTTLE(10, "Status: %.8X", mStatus);
00300                 }
00301 }
00302