Gary Servin
/
rosserial_mbed_lib
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
rosserial_mbed_lib
by 
nucho
Embed:
(wiki syntax)
Show/hide line numbers
NavSatFix.h
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 virtual const char * getType(){ return "sensor_msgs/NavSatFix"; }; 00156 virtual const char * getMD5(){ return "2d3a8cd499b9b4a0249fb98fd05cfa48"; }; 00157 00158 }; 00159 00160 } 00161 #endif
Generated on Tue Jul 12 2022 19:53:57 by
1.7.2