void IMU::UpdateData(IMU_DATA &data) { XsByteArray imuData; XsMessageArray msgs; while (msgs.empty()) { Aris::Core::Sleep(1); pDevice->readDataToBuffer(imuData); pDevice->processBufferedData(imuData, msgs); } //std::cout << "msg num:" << msgs.size()<<std::endl; for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it) { // Retrieve a packet XsDataPacket packet; if ((*it).getMessageId() == XMID_MtData2) { packet.setMessage((*it)); packet.setDeviceId(pDevice->mtPort.deviceId()); } // Get the all data auto eul = packet.orientationEuler(); auto sdi = packet.sdiData(); auto acc = packet.calibratedAcceleration(); data.yaw = eul.yaw()*PI / 180; data.pitch = eul.pitch()*PI / 180; data.roll = eul.roll()*PI / 180; data.va = sdi.orientationIncrement().x() * 2 * 100; data.vb = sdi.orientationIncrement().y() * 2 * 100; data.vc = sdi.orientationIncrement().z() * 2 * 100; std::copy_n(acc.data(), acc.size(), data.acc); data.time = packet.timeOfArrival().nowMs(); data.pmLhs = *pDevice->pmImuGround2BodyGround; data.pmRhs = *pDevice->pmBody2Imu; } msgs.clear(); }
void ofxXsens::update() { if (callback.packetAvailable()) { // Retrieve a packet XsDataPacket packet = callback.getNextPacket(); //Get the quaternion data XsQuaternion quaternion = packet.orientationQuaternion(); ofLogVerbose("ofxXsens") << "\r" << "q0:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_w << ",q1:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_x << ",q2:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_y << ",q3:" << std::setw(5) << std::fixed << std::setprecision(2) << quaternion.m_z ; // Convert packet to euler euler = packet.orientationEuler(); ofLogVerbose("ofxXsens") << ",Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll << ",Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch << ",Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw ; // GPS if(packet.containsLatitudeLongitude()) { lat_long = packet.latitudeLongitude(); ofLogVerbose("ofxXsens") << "\n" << "Latitude:" << std::setprecision(9) << lat_long[0] << " Longitude:" << lat_long[1]; } else if(packet.containsPositionLLA()) { ofLogVerbose("ofxXsens") << "\n" << "Packet contains position LLA data" ; } else if(packet.containsRawGpsSol()){ ofLogVerbose("ofxXsens") << "\n" << "Packet contains RAW GPS data" ; } } }
/*------------------------------------------------------------- doProcess -------------------------------------------------------------*/ void CIMUXSens_MT4::doProcess() { #if MRPT_HAS_xSENS_MT4 if(m_state == ssError) { mrpt::system::sleep(200); initialize(); } if(m_state == ssError) return; XsByteArray data; XsMessageArray msgs; my_xsens_device.readDataToBuffer(data); my_xsens_device.processBufferedData(data, msgs); for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it) { // Retrieve a packet XsDataPacket packet; if ((*it).getMessageId() == XMID_MtData) { LegacyDataPacket lpacket(1, false); lpacket.setMessage((*it)); lpacket.setXbusSystem(false, false); lpacket.setDeviceId(my_xsens_devid, 0); lpacket.setDataFormat(XOM_Orientation, XOS_OrientationMode_Euler | XOS_Timestamp_PacketCounter | XOS_CalibratedMode_All/*XOS_OrientationMode_Quaternion*/,0); //lint !e534 XsDataPacket_assignFromXsLegacyDataPacket(&packet, &lpacket, 0); } else if ((*it).getMessageId() == XMID_MtData2) { packet.setMessage((*it)); packet.setDeviceId(my_xsens_devid); } // Data properly collected: extract data fields // ------------------------------------------------- m_state = ssWorking; CObservationIMUPtr obs = CObservationIMU::Create(); if (packet.containsOrientation()) { XsEuler euler = packet.orientationEuler(); obs->rawMeasurements[IMU_YAW] = DEG2RAD(euler.yaw()); obs->dataIsPresent[IMU_YAW] = true; obs->rawMeasurements[IMU_PITCH] = DEG2RAD(euler.pitch()); obs->dataIsPresent[IMU_PITCH] = true; obs->rawMeasurements[IMU_ROLL] = DEG2RAD(euler.roll()); obs->dataIsPresent[IMU_ROLL] = true; XsQuaternion quat = packet.orientationQuaternion(); obs->rawMeasurements[IMU_ORI_QUAT_X] = quat.x(); obs->dataIsPresent[IMU_ORI_QUAT_X] = true; obs->rawMeasurements[IMU_ORI_QUAT_Y] = quat.y(); obs->dataIsPresent[IMU_ORI_QUAT_Y] = true; obs->rawMeasurements[IMU_ORI_QUAT_Z] = quat.z(); obs->dataIsPresent[IMU_ORI_QUAT_Z] = true; obs->rawMeasurements[IMU_ORI_QUAT_W] = quat.w(); obs->dataIsPresent[IMU_ORI_QUAT_W] = true; } if (packet.containsCalibratedAcceleration()) { XsVector acc_data = packet.calibratedAcceleration(); obs->rawMeasurements[IMU_X_ACC] = acc_data[0]; obs->dataIsPresent[IMU_X_ACC] = true; obs->rawMeasurements[IMU_Y_ACC] = acc_data[1]; obs->dataIsPresent[IMU_Y_ACC] = true; obs->rawMeasurements[IMU_Z_ACC] = acc_data[2]; obs->dataIsPresent[IMU_Z_ACC] = true; } if (packet.containsCalibratedGyroscopeData()) { XsVector gyr_data = packet.calibratedGyroscopeData(); obs->rawMeasurements[IMU_YAW_VEL] = gyr_data[2]; obs->dataIsPresent[IMU_YAW_VEL] = true; obs->rawMeasurements[IMU_PITCH_VEL] = gyr_data[1]; obs->dataIsPresent[IMU_PITCH_VEL] = true; obs->rawMeasurements[IMU_ROLL_VEL] = gyr_data[0]; obs->dataIsPresent[IMU_ROLL_VEL] = true; } if (packet.containsCalibratedMagneticField()) { XsVector mag_data = packet.calibratedMagneticField(); obs->rawMeasurements[IMU_MAG_X] = mag_data[0]; obs->dataIsPresent[IMU_MAG_X] = true; obs->rawMeasurements[IMU_MAG_Y] = mag_data[1]; obs->dataIsPresent[IMU_MAG_Y] = true; obs->rawMeasurements[IMU_MAG_Z] = mag_data[2]; obs->dataIsPresent[IMU_MAG_Z] = true; } if (packet.containsVelocity()) { XsVector vel_data = packet.velocity(); obs->rawMeasurements[IMU_X_VEL] = vel_data[0]; obs->dataIsPresent[IMU_X_VEL] = true; obs->rawMeasurements[IMU_Y_VEL] = vel_data[1]; obs->dataIsPresent[IMU_Y_VEL] = true; obs->rawMeasurements[IMU_Z_VEL] = vel_data[2]; obs->dataIsPresent[IMU_Z_VEL] = true; } if (packet.containsTemperature()) { obs->rawMeasurements[IMU_TEMPERATURE] = packet.temperature(); obs->dataIsPresent[IMU_TEMPERATURE] = true; } if (packet.containsAltitude()) { obs->rawMeasurements[IMU_ALTITUDE ] = packet.altitude(); obs->dataIsPresent[IMU_ALTITUDE ] = true; } // TimeStamp if (packet.containsSampleTime64()) { const uint64_t nowUI = packet.sampleTime64(); uint64_t AtUI = 0; if( m_timeStartUI == 0 ) { m_timeStartUI = nowUI; m_timeStartTT = mrpt::system::now(); } else AtUI = nowUI - m_timeStartUI; double AtDO = AtUI * 1000.0; // Difference in intervals of 100 nsecs obs->timestamp = m_timeStartTT + AtDO; } else if (packet.containsUtcTime()) { XsUtcTime utc = packet.utcTime(); mrpt::system::TTimeParts parts; parts.day_of_week = 0; parts.daylight_saving = 0; parts.year = utc.m_year; parts.month = utc.m_month; parts.day = utc.m_day; parts.hour = utc.m_hour; parts.minute = utc.m_minute; parts.second = utc.m_second + (utc.m_nano * 1000000000.0); obs->timestamp = mrpt::system::buildTimestampFromParts(parts); } else obs->timestamp = mrpt::system::now(); obs->sensorPose = m_sensorPose; obs->sensorLabel = m_sensorLabel; appendObservation(obs); if (packet.containsLatitudeLongitude()) { XsVector lla_data = packet.latitudeLongitude(); CObservationGPSPtr obsGPS = CObservationGPSPtr( new CObservationGPS() ); CObservationGPS::TGPSDatum_RMC& rGPS = obsGPS->RMC_datum; rGPS.latitude_degrees = lla_data[0]; rGPS.longitude_degrees = lla_data[1]; if (packet.containsStatus() && packet.status() & XSF_GpsValid) rGPS.validity_char = 'A'; else rGPS.validity_char = 'V'; if (packet.containsUtcTime()) { XsUtcTime utc = packet.utcTime(); rGPS.UTCTime.hour = utc.m_hour; rGPS.UTCTime.minute = utc.m_minute; rGPS.UTCTime.sec = utc.m_second + (utc.m_nano * 1000000.0); } else { rGPS.UTCTime.hour = ((obs->timestamp / (60 * 60 * ((uint64_t)1000000 / 100))) % 24); rGPS.UTCTime.minute = ((obs->timestamp / (60 * ((uint64_t)1000000 / 100))) % 60); rGPS.UTCTime.sec = fmod(obs->timestamp / (1000000.0 / 100), 60); } obsGPS->has_RMC_datum = true; obsGPS->timestamp = obs->timestamp; obsGPS->sensorPose = m_sensorPose; obsGPS->sensorLabel = m_sensorLabel; if (packet.containsVelocity()) { XsVector vel_data = packet.velocity(); rGPS.speed_knots = sqrt(vel_data[0] * vel_data[0] + vel_data[1] * vel_data[1]); rGPS.direction_degrees = 0; //Could be worked out from velocity and magnatic field perhaps. } else rGPS.speed_knots = rGPS.direction_degrees = 0; appendObservation(obsGPS); } std::cout << std::flush; } msgs.clear(); #else THROW_EXCEPTION("MRPT has been compiled with 'BUILD_XSENS_MT4'=OFF, so this class cannot be used."); #endif }
bool XSensINS::Iterate() { AppCastingMOOSApp::Iterate(); // Read Data XsByteArray data; XsMessageArray msgs; m_device.readDataToBuffer(data); m_device.processBufferedData(data, msgs); for (XsMessageArray::iterator it = msgs.begin(); it != msgs.end(); ++it) { // Retrieve a packet XsDataPacket packet; packet.setMessage((*it)); // Convert packet to Euler if(packet.containsOrientation()){ m_euler = packet.orientationEuler(); m_nav_heading = m_euler.yaw(); Notify("IMU_PITCH", m_euler.pitch()); Notify("IMU_ROLL", m_euler.roll()); Notify("NAV_HEADING", m_nav_heading); } // Acceleration if(packet.containsCalibratedAcceleration()){ m_acceleration = packet.calibratedAcceleration(); Notify("IMU_ACC_X", m_acceleration[0]); Notify("IMU_ACC_Y", m_acceleration[1]); Notify("IMU_ACC_Z", m_acceleration[2]); } //Gyro if(packet.containsCalibratedGyroscopeData()){ m_gyro = packet.calibratedGyroscopeData(); Notify("IMU_GYR_X", m_gyro[0]*180.0/M_PI); Notify("IMU_GYR_Y", m_gyro[1]*180.0/M_PI); Notify("IMU_GYR_Z", m_gyro[2]*180.0/M_PI); } //Magneto if(packet.containsCalibratedMagneticField()){ m_mag = packet.calibratedMagneticField(); Notify("IMU_MAG_X", m_mag[0]); Notify("IMU_MAG_Y", m_mag[1]); Notify("IMU_MAG_Z", m_mag[2]); Notify("IMU_MAG_N", sqrt(pow(m_mag[0], 2) + pow(m_mag[1], 2) + pow(m_mag[2], 2))); } //LatLon if(packet.containsLatitudeLongitude()){ m_latlon = packet.latitudeLongitude(); Notify("INS_LAT", m_latlon[0]); Notify("INS_LONG", m_latlon[1]); } //Velocity if(packet.containsVelocity()){ m_velocity = packet.velocity(); m_velocity_norm = sqrt(pow(m_velocity[0], 2) + pow(m_velocity[1], 2) + pow(m_velocity[2], 2)); Notify("NAV_SPEED", m_velocity_norm); } } msgs.clear(); AppCastingMOOSApp::PostReport(); return true; }
/*! \brief Create 64 bit counter for a packet. \details Wrap when new XsDataPacket is too far away from the previous XsDataPacket in time. Use half cache size as reasonable time difference When infinite cache, simply wrap when new is lower than old \param pack The XsDataPacket that needs its 64-bit sample counter updated \param highestPacket The highest packet available for the current device, it will be updated if the new counter is higher than the stored value. \returns The computed counter for the packet. */ int64_t PacketStamper::stampPacket(XsDataPacket& pack, XsDataPacket& highestPacket) { // int did = 0; // if (pack.containsMtwSdiData()) // { // MtwSdiData sdi = pack.mtwSdiData(); // did = sdi.m_deviceId; // JLDEBUG(gJournal, "XsensDeviceAPI", "%s [%08x] SDI interval (%d-%d)\n", __FUNCTION__, sdi.m_deviceId, sdi.m_firstFrameNumber, sdi.m_lastFrameNumber); // } //! \todo This could be a (couple of) milliseconds too late, this should be set as soon as the source message arrives: mantis 7157 pack.setTimeOfArrival(XsTimeStamp::now()); int64_t newCounter, lastCounter = -1; if (!highestPacket.empty()) lastCounter = highestPacket.packetId().msTime(); if (pack.containsPacketCounter()) newCounter = calculateLargePacketCounter(pack.packetCounter(), lastCounter); else if (pack.containsSampleTimeFine()) { if (pack.containsSampleTimeCoarse()) newCounter = (int64_t) pack.sampleTime64(); else newCounter = calculateLargeSampleTime((int32_t) pack.sampleTimeFine(), lastCounter); } else if (pack.containsPacketCounter8()) newCounter = calculateLargePacketCounter8(pack.packetCounter8(), lastCounter); else newCounter = lastCounter + 1; // JLDEBUG(gJournal, "XsensDeviceAPI", "%s [%08x] old = %I64d new = %I64d diff = %I64d\n", __FUNCTION__, did, lastCounter, newCounter, (newCounter-lastCounter)); pack.setPacketId(newCounter); if (newCounter > lastCounter) highestPacket = pack; return newCounter; }
/** * \brief Fills data using XsDataPacket object * @param _packet Incoming packet from Xsens device. */ void SensorData::fillData(XsDataPacket * _packet){ XsDataPacket packet = *_packet; XsMessage msg = packet.toMessage(); XsSize msg_size = msg.getDataSize(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Data Size: " << msg_size); // Get the orientation data if (packet.containsOrientation()) { XsQuaternion quaternion = packet.orientationQuaternion(); q1 = quaternion.m_x; q2 = quaternion.m_y; q3 = quaternion.m_z; q0 = quaternion.m_w; XsEuler euler = packet.orientationEuler(); eroll = euler.m_roll; epitch = euler.m_pitch; eyaw = euler.m_yaw; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE,"Orientation: Roll:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_roll << ", Pitch:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_pitch << ", Yaw:" << std::setw(7) << std::fixed << std::setprecision(2) << euler.m_yaw); } // Get the gyroscope data if (packet.containsCalibratedGyroscopeData()) { XsVector gyroscope = packet.calibratedGyroscopeData(); gyrX = gyroscope.at(0); gyrY = gyroscope.at(1); gyrZ = gyroscope.at(2); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Angular Velocity: ( " << gyrX << ", " << gyrY << ", " << gyrZ << ")" ); } // Get the acceleration data if (packet.containsCalibratedAcceleration()) { XsVector acceleration = packet.calibratedAcceleration(); accX = acceleration.at(0); accY = acceleration.at(1); accZ = acceleration.at(2); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Linear Acceleration: (" << accX << "," << accX << "," << accZ << ")" ); } //Get the altitude data if(packet.containsAltitude()){ mAltitude = packet.altitude(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsAltitude: "); } //Get the Latitude and Longitude Data if(packet.containsLatitudeLongitude()){ XsVector latlon = packet.latitudeLongitude(); mLatitude = latlon[0]; mLongitude = latlon[1]; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "containsLatitudeLongitude"); } // Get the magnetic field data if (packet.containsCalibratedMagneticField()) { XsVector magneticField = packet.calibratedMagneticField(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Magnetic Field: ("<< magneticField[0] <<" , " << magneticField[1] << " , "<< magneticField[2] << ")"); magX = magneticField.at(0); magY = magneticField.at(1); magZ = magneticField.at(2); } // Get Temperature data if (packet.containsTemperature()){ double temperature = packet.temperature(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Temperature : " << temperature); mTemperature = temperature; } // Get Pressure Data if(packet.containsPressure() ){ XsPressure pressure = packet.pressure(); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pressure : " << pressure.m_pressure); mPressure= pressure.m_pressure; } // Get Velocity Data if( packet.containsVelocity() ){ XsVector velocity = packet.velocity(); mVelocityX = velocity.at(0); mVelocityY = velocity.at(1); mVelocityZ = velocity.at(2); ROS_INFO_STREAM(" Velocity [ x (east) : " << mVelocityX << ", y (north) : " << mVelocityY << ", z (up) " << mVelocityZ << " ]"); } // Get GPS PVT Data if( packet.containsGpsPvtData() ){ XsGpsPvtData gpsPvtData = packet.gpsPvtData(); ROS_INFO_STREAM(" Horizontal accuracy: " << gpsPvtData.m_hacc ); } if(packet.containsRawAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Acceleration") ;} if(packet.containsRawGyroscopeData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Raw Gyroscope") ;} if(packet.containsRawGyroscopeTemperatureData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawGyrTemp");} if(packet.containsRawMagneticField() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawMag");} if(packet.containsRawTemperature() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawTemp");} if(packet.containsRawData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RawData");} if(packet.containsCalibratedData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Calib Data");} if(packet.containsSdiData() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SDI");} //if(packet.containsStatusByte() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "StatusByte");} if(packet.containsDetailedStatus() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "DetailedStatus");} if(packet.containsPacketCounter8() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter8");} if(packet.containsPacketCounter() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PacketCounter");} if(packet.containsSampleTimeFine() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeFine");} if(packet.containsSampleTimeCoarse() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTimeCoarse");} if(packet.containsSampleTime64() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SampleTime64");} if(packet.containsFreeAcceleration() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "FreeAcceleration");} if(packet.containsPressureAge() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "PressureAge");} if(packet.containsAnalogIn1Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN1IN");} if(packet.containsAnalogIn2Data() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "AN2IN");} if(packet.containsPositionLLA() ){ XsVector posLLA = packet.positionLLA(); mLatitude = posLLA[0]; mLongitude = posLLA[1]; mAltitude = posLLA[2]; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLatitude = " << mLatitude); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mLongitude = " << mLongitude); ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "mAltitude = " << mAltitude); //for(int i=0; i < posLLA.size(); i++){ //ROS_INFO_STREAM("posLLA[" << i << "] : " << posLLA[i] ); //} } if(packet.containsUtcTime() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "UTC-Time");} if(packet.containsFrameRange() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "Frame Range");} if(packet.containsRssi() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "RSSI");} if(packet.containsRawGpsDop() ){ //ROS_INFO_THROTTLE(THROTTLE_VALUE, "DOP");} XsRawGpsDop dop = packet.rawGpsDop(); m_gdop = ((float) dop.m_gdop)/100; m_pdop = ((float) dop.m_pdop)/100; m_tdop = ((float) dop.m_tdop)/100; m_vdop = ((float) dop.m_vdop)/100; m_hdop = ((float) dop.m_hdop)/100; m_edop = ((float) dop.m_edop)/100; m_ndop = ((float) dop.m_ndop)/100; m_itow = ((float) dop.m_itow)/1000; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "GDOP = " << m_gdop << ",PDOP = " << m_pdop << ",TDOP = " << m_tdop << ",VDOP = " << m_vdop << ",HDOP = " << m_hdop << ",EDOP = " << m_edop << ",NDOP = " << m_ndop << ",ITOW = " << m_itow); } if(packet.containsRawGpsSol() ){ // ROS_INFO_THROTTLE(THROTTLE_VALUE, "SOL");} XsRawGpsSol sol = packet.rawGpsSol(); mPositionAccuracy = sol.m_pacc; mSpeedAccuracy = sol.m_sacc; mSatelliteNumber = sol.m_numsv; mGpsFixStatus = sol.m_gpsfix; gpsVelocityX=sol.m_ecef_vx/100; gpsVelocityY=sol.m_ecef_vy/100; gpsVelocityZ=sol.m_ecef_vz/100; ROS_INFO_STREAM_THROTTLE(THROTTLE_VALUE, "Pos Acc = " << mPositionAccuracy << ",Speed Acc = " << mSpeedAccuracy << ",Sat Number = " << mSatelliteNumber << ",GPS FIX = " << std::hex << mGpsFixStatus); } if(packet.containsRawGpsTimeUtc() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "GPS-UTC");} if(packet.containsRawGpsSvInfo() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "SV INFO");} // if(packet.containsTriggerIndication() ){ ROS_INFO_THROTTLE(THROTTLE_VALUE, "TRIGGER");} //Get Status byte if( packet.containsStatus() ){ mStatus = packet.status(); ROS_INFO_THROTTLE(10, "Status: %.8X", mStatus); } }