コード例 #1
0
ファイル: ofxXsens.cpp プロジェクト: pierrep/ofxXsens
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" ;
            }

    }

}
コード例 #2
0
/**
 * \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);
		}
}