Go to the documentation of this file.00001 #ifndef _ROS_sensor_msgs_NavSatFix_h
00002 #define _ROS_sensor_msgs_NavSatFix_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "std_msgs/Header.h"
00009 #include "sensor_msgs/NavSatStatus.h"
00010
00011 namespace sensor_msgs
00012 {
00013
00014 class NavSatFix : public ros::Msg
00015 {
00016 public:
00017 std_msgs::Header header;
00018 sensor_msgs::NavSatStatus status;
00019 float latitude;
00020 float longitude;
00021 float altitude;
00022 float position_covariance[9];
00023 uint8_t position_covariance_type;
00024 enum { COVARIANCE_TYPE_UNKNOWN = 0 };
00025 enum { COVARIANCE_TYPE_APPROXIMATED = 1 };
00026 enum { COVARIANCE_TYPE_DIAGONAL_KNOWN = 2 };
00027 enum { COVARIANCE_TYPE_KNOWN = 3 };
00028
00029 virtual int serialize(unsigned char *outbuffer) const
00030 {
00031 int offset = 0;
00032 offset += this->header.serialize(outbuffer + offset);
00033 offset += this->status.serialize(outbuffer + offset);
00034 int32_t * val_latitude = (int32_t *) &(this->latitude);
00035 int32_t exp_latitude = (((*val_latitude)>>23)&255);
00036 if(exp_latitude != 0)
00037 exp_latitude += 1023-127;
00038 int32_t sig_latitude = *val_latitude;
00039 *(outbuffer + offset++) = 0;
00040 *(outbuffer + offset++) = 0;
00041 *(outbuffer + offset++) = 0;
00042 *(outbuffer + offset++) = (sig_latitude<<5) & 0xff;
00043 *(outbuffer + offset++) = (sig_latitude>>3) & 0xff;
00044 *(outbuffer + offset++) = (sig_latitude>>11) & 0xff;
00045 *(outbuffer + offset++) = ((exp_latitude<<4) & 0xF0) | ((sig_latitude>>19)&0x0F);
00046 *(outbuffer + offset++) = (exp_latitude>>4) & 0x7F;
00047 if(this->latitude < 0) *(outbuffer + offset -1) |= 0x80;
00048 int32_t * val_longitude = (int32_t *) &(this->longitude);
00049 int32_t exp_longitude = (((*val_longitude)>>23)&255);
00050 if(exp_longitude != 0)
00051 exp_longitude += 1023-127;
00052 int32_t sig_longitude = *val_longitude;
00053 *(outbuffer + offset++) = 0;
00054 *(outbuffer + offset++) = 0;
00055 *(outbuffer + offset++) = 0;
00056 *(outbuffer + offset++) = (sig_longitude<<5) & 0xff;
00057 *(outbuffer + offset++) = (sig_longitude>>3) & 0xff;
00058 *(outbuffer + offset++) = (sig_longitude>>11) & 0xff;
00059 *(outbuffer + offset++) = ((exp_longitude<<4) & 0xF0) | ((sig_longitude>>19)&0x0F);
00060 *(outbuffer + offset++) = (exp_longitude>>4) & 0x7F;
00061 if(this->longitude < 0) *(outbuffer + offset -1) |= 0x80;
00062 int32_t * val_altitude = (int32_t *) &(this->altitude);
00063 int32_t exp_altitude = (((*val_altitude)>>23)&255);
00064 if(exp_altitude != 0)
00065 exp_altitude += 1023-127;
00066 int32_t sig_altitude = *val_altitude;
00067 *(outbuffer + offset++) = 0;
00068 *(outbuffer + offset++) = 0;
00069 *(outbuffer + offset++) = 0;
00070 *(outbuffer + offset++) = (sig_altitude<<5) & 0xff;
00071 *(outbuffer + offset++) = (sig_altitude>>3) & 0xff;
00072 *(outbuffer + offset++) = (sig_altitude>>11) & 0xff;
00073 *(outbuffer + offset++) = ((exp_altitude<<4) & 0xF0) | ((sig_altitude>>19)&0x0F);
00074 *(outbuffer + offset++) = (exp_altitude>>4) & 0x7F;
00075 if(this->altitude < 0) *(outbuffer + offset -1) |= 0x80;
00076 for( uint8_t i = 0; i < 9; i++){
00077 int32_t * val_position_covariancei = (int32_t *) &(this->position_covariance[i]);
00078 int32_t exp_position_covariancei = (((*val_position_covariancei)>>23)&255);
00079 if(exp_position_covariancei != 0)
00080 exp_position_covariancei += 1023-127;
00081 int32_t sig_position_covariancei = *val_position_covariancei;
00082 *(outbuffer + offset++) = 0;
00083 *(outbuffer + offset++) = 0;
00084 *(outbuffer + offset++) = 0;
00085 *(outbuffer + offset++) = (sig_position_covariancei<<5) & 0xff;
00086 *(outbuffer + offset++) = (sig_position_covariancei>>3) & 0xff;
00087 *(outbuffer + offset++) = (sig_position_covariancei>>11) & 0xff;
00088 *(outbuffer + offset++) = ((exp_position_covariancei<<4) & 0xF0) | ((sig_position_covariancei>>19)&0x0F);
00089 *(outbuffer + offset++) = (exp_position_covariancei>>4) & 0x7F;
00090 if(this->position_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00091 }
00092 *(outbuffer + offset + 0) = (this->position_covariance_type >> (8 * 0)) & 0xFF;
00093 offset += sizeof(this->position_covariance_type);
00094 return offset;
00095 }
00096
00097 virtual int deserialize(unsigned char *inbuffer)
00098 {
00099 int offset = 0;
00100 offset += this->header.deserialize(inbuffer + offset);
00101 offset += this->status.deserialize(inbuffer + offset);
00102 uint32_t * val_latitude = (uint32_t*) &(this->latitude);
00103 offset += 3;
00104 *val_latitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00105 *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00106 *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00107 *val_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00108 uint32_t exp_latitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00109 exp_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00110 if(exp_latitude !=0)
00111 *val_latitude |= ((exp_latitude)-1023+127)<<23;
00112 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->latitude = -this->latitude;
00113 uint32_t * val_longitude = (uint32_t*) &(this->longitude);
00114 offset += 3;
00115 *val_longitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00116 *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00117 *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00118 *val_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00119 uint32_t exp_longitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00120 exp_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00121 if(exp_longitude !=0)
00122 *val_longitude |= ((exp_longitude)-1023+127)<<23;
00123 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->longitude = -this->longitude;
00124 uint32_t * val_altitude = (uint32_t*) &(this->altitude);
00125 offset += 3;
00126 *val_altitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00127 *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00128 *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00129 *val_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00130 uint32_t exp_altitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00131 exp_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00132 if(exp_altitude !=0)
00133 *val_altitude |= ((exp_altitude)-1023+127)<<23;
00134 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->altitude = -this->altitude;
00135 for( uint8_t i = 0; i < 9; i++){
00136 uint32_t * val_position_covariancei = (uint32_t*) &(this->position_covariance[i]);
00137 offset += 3;
00138 *val_position_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00139 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00140 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00141 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00142 uint32_t exp_position_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00143 exp_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00144 if(exp_position_covariancei !=0)
00145 *val_position_covariancei |= ((exp_position_covariancei)-1023+127)<<23;
00146 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->position_covariance[i] = -this->position_covariance[i];
00147 }
00148 this->position_covariance_type = ((uint8_t) (*(inbuffer + offset)));
00149 offset += sizeof(this->position_covariance_type);
00150 return offset;
00151 }
00152
00153 const char * getType(){ return "sensor_msgs/NavSatFix"; };
00154 const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; };
00155
00156 };
00157
00158 }
00159 #endif