Exemple #1
0
/*! \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);
		}
}