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 = (long *) &(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 = (long *) &(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 = (long *) &(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 unsigned char * position_covariance_val = (unsigned char *) this->position_covariance;
00077 for( uint8_t i = 0; i < 9; i++){
00078 int32_t * val_position_covariancei = (long *) &(this->position_covariance[i]);
00079 int32_t exp_position_covariancei = (((*val_position_covariancei)>>23)&255);
00080 if(exp_position_covariancei != 0)
00081 exp_position_covariancei += 1023-127;
00082 int32_t sig_position_covariancei = *val_position_covariancei;
00083 *(outbuffer + offset++) = 0;
00084 *(outbuffer + offset++) = 0;
00085 *(outbuffer + offset++) = 0;
00086 *(outbuffer + offset++) = (sig_position_covariancei<<5) & 0xff;
00087 *(outbuffer + offset++) = (sig_position_covariancei>>3) & 0xff;
00088 *(outbuffer + offset++) = (sig_position_covariancei>>11) & 0xff;
00089 *(outbuffer + offset++) = ((exp_position_covariancei<<4) & 0xF0) | ((sig_position_covariancei>>19)&0x0F);
00090 *(outbuffer + offset++) = (exp_position_covariancei>>4) & 0x7F;
00091 if(this->position_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80;
00092 }
00093 *(outbuffer + offset + 0) = (this->position_covariance_type >> (8 * 0)) & 0xFF;
00094 offset += sizeof(this->position_covariance_type);
00095 return offset;
00096 }
00097
00098 virtual int deserialize(unsigned char *inbuffer)
00099 {
00100 int offset = 0;
00101 offset += this->header.deserialize(inbuffer + offset);
00102 offset += this->status.deserialize(inbuffer + offset);
00103 uint32_t * val_latitude = (uint32_t*) &(this->latitude);
00104 offset += 3;
00105 *val_latitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00106 *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00107 *val_latitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00108 *val_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00109 uint32_t exp_latitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00110 exp_latitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00111 if(exp_latitude !=0)
00112 *val_latitude |= ((exp_latitude)-1023+127)<<23;
00113 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->latitude = -this->latitude;
00114 uint32_t * val_longitude = (uint32_t*) &(this->longitude);
00115 offset += 3;
00116 *val_longitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00117 *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00118 *val_longitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00119 *val_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00120 uint32_t exp_longitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00121 exp_longitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00122 if(exp_longitude !=0)
00123 *val_longitude |= ((exp_longitude)-1023+127)<<23;
00124 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->longitude = -this->longitude;
00125 uint32_t * val_altitude = (uint32_t*) &(this->altitude);
00126 offset += 3;
00127 *val_altitude = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00128 *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00129 *val_altitude |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00130 *val_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00131 uint32_t exp_altitude = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00132 exp_altitude |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00133 if(exp_altitude !=0)
00134 *val_altitude |= ((exp_altitude)-1023+127)<<23;
00135 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->altitude = -this->altitude;
00136 uint8_t * position_covariance_val = (uint8_t*) this->position_covariance;
00137 for( uint8_t i = 0; i < 9; i++){
00138 uint32_t * val_position_covariancei = (uint32_t*) &(this->position_covariance[i]);
00139 offset += 3;
00140 *val_position_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00141 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00142 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00143 *val_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00144 uint32_t exp_position_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00145 exp_position_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00146 if(exp_position_covariancei !=0)
00147 *val_position_covariancei |= ((exp_position_covariancei)-1023+127)<<23;
00148 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->position_covariance[i] = -this->position_covariance[i];
00149 }
00150 this->position_covariance_type |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00151 offset += sizeof(this->position_covariance_type);
00152 return offset;
00153 }
00154
00155 const char * getType(){ return "sensor_msgs/NavSatFix"; };
00156 const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; };
00157
00158 };
00159
00160 }
00161 #endif
