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
Imu.h
00001 #ifndef _ROS_sensor_msgs_Imu_h 00002 #define _ROS_sensor_msgs_Imu_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 "geometry_msgs/Quaternion.h" 00010 #include "geometry_msgs/Vector3.h" 00011 00012 namespace sensor_msgs 00013 { 00014 00015 class Imu : public ros::Msg 00016 { 00017 public: 00018 std_msgs::Header header; 00019 geometry_msgs::Quaternion orientation; 00020 float orientation_covariance[9]; 00021 geometry_msgs::Vector3 angular_velocity; 00022 float angular_velocity_covariance[9]; 00023 geometry_msgs::Vector3 linear_acceleration; 00024 float linear_acceleration_covariance[9]; 00025 00026 virtual int serialize(unsigned char *outbuffer) const 00027 { 00028 int offset = 0; 00029 offset += this->header.serialize(outbuffer + offset); 00030 offset += this->orientation.serialize(outbuffer + offset); 00031 unsigned char * orientation_covariance_val = (unsigned char *) this->orientation_covariance; 00032 for( uint8_t i = 0; i < 9; i++){ 00033 int32_t * val_orientation_covariancei = (long *) &(this->orientation_covariance[i]); 00034 int32_t exp_orientation_covariancei = (((*val_orientation_covariancei)>>23)&255); 00035 if(exp_orientation_covariancei != 0) 00036 exp_orientation_covariancei += 1023-127; 00037 int32_t sig_orientation_covariancei = *val_orientation_covariancei; 00038 *(outbuffer + offset++) = 0; 00039 *(outbuffer + offset++) = 0; 00040 *(outbuffer + offset++) = 0; 00041 *(outbuffer + offset++) = (sig_orientation_covariancei<<5) & 0xff; 00042 *(outbuffer + offset++) = (sig_orientation_covariancei>>3) & 0xff; 00043 *(outbuffer + offset++) = (sig_orientation_covariancei>>11) & 0xff; 00044 *(outbuffer + offset++) = ((exp_orientation_covariancei<<4) & 0xF0) | ((sig_orientation_covariancei>>19)&0x0F); 00045 *(outbuffer + offset++) = (exp_orientation_covariancei>>4) & 0x7F; 00046 if(this->orientation_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80; 00047 } 00048 offset += this->angular_velocity.serialize(outbuffer + offset); 00049 unsigned char * angular_velocity_covariance_val = (unsigned char *) this->angular_velocity_covariance; 00050 for( uint8_t i = 0; i < 9; i++){ 00051 int32_t * val_angular_velocity_covariancei = (long *) &(this->angular_velocity_covariance[i]); 00052 int32_t exp_angular_velocity_covariancei = (((*val_angular_velocity_covariancei)>>23)&255); 00053 if(exp_angular_velocity_covariancei != 0) 00054 exp_angular_velocity_covariancei += 1023-127; 00055 int32_t sig_angular_velocity_covariancei = *val_angular_velocity_covariancei; 00056 *(outbuffer + offset++) = 0; 00057 *(outbuffer + offset++) = 0; 00058 *(outbuffer + offset++) = 0; 00059 *(outbuffer + offset++) = (sig_angular_velocity_covariancei<<5) & 0xff; 00060 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>3) & 0xff; 00061 *(outbuffer + offset++) = (sig_angular_velocity_covariancei>>11) & 0xff; 00062 *(outbuffer + offset++) = ((exp_angular_velocity_covariancei<<4) & 0xF0) | ((sig_angular_velocity_covariancei>>19)&0x0F); 00063 *(outbuffer + offset++) = (exp_angular_velocity_covariancei>>4) & 0x7F; 00064 if(this->angular_velocity_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80; 00065 } 00066 offset += this->linear_acceleration.serialize(outbuffer + offset); 00067 unsigned char * linear_acceleration_covariance_val = (unsigned char *) this->linear_acceleration_covariance; 00068 for( uint8_t i = 0; i < 9; i++){ 00069 int32_t * val_linear_acceleration_covariancei = (long *) &(this->linear_acceleration_covariance[i]); 00070 int32_t exp_linear_acceleration_covariancei = (((*val_linear_acceleration_covariancei)>>23)&255); 00071 if(exp_linear_acceleration_covariancei != 0) 00072 exp_linear_acceleration_covariancei += 1023-127; 00073 int32_t sig_linear_acceleration_covariancei = *val_linear_acceleration_covariancei; 00074 *(outbuffer + offset++) = 0; 00075 *(outbuffer + offset++) = 0; 00076 *(outbuffer + offset++) = 0; 00077 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei<<5) & 0xff; 00078 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>3) & 0xff; 00079 *(outbuffer + offset++) = (sig_linear_acceleration_covariancei>>11) & 0xff; 00080 *(outbuffer + offset++) = ((exp_linear_acceleration_covariancei<<4) & 0xF0) | ((sig_linear_acceleration_covariancei>>19)&0x0F); 00081 *(outbuffer + offset++) = (exp_linear_acceleration_covariancei>>4) & 0x7F; 00082 if(this->linear_acceleration_covariance[i] < 0) *(outbuffer + offset -1) |= 0x80; 00083 } 00084 return offset; 00085 } 00086 00087 virtual int deserialize(unsigned char *inbuffer) 00088 { 00089 int offset = 0; 00090 offset += this->header.deserialize(inbuffer + offset); 00091 offset += this->orientation.deserialize(inbuffer + offset); 00092 uint8_t * orientation_covariance_val = (uint8_t*) this->orientation_covariance; 00093 for( uint8_t i = 0; i < 9; i++){ 00094 uint32_t * val_orientation_covariancei = (uint32_t*) &(this->orientation_covariance[i]); 00095 offset += 3; 00096 *val_orientation_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00097 *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00098 *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00099 *val_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00100 uint32_t exp_orientation_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00101 exp_orientation_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00102 if(exp_orientation_covariancei !=0) 00103 *val_orientation_covariancei |= ((exp_orientation_covariancei)-1023+127)<<23; 00104 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->orientation_covariance[i] = -this->orientation_covariance[i]; 00105 } 00106 offset += this->angular_velocity.deserialize(inbuffer + offset); 00107 uint8_t * angular_velocity_covariance_val = (uint8_t*) this->angular_velocity_covariance; 00108 for( uint8_t i = 0; i < 9; i++){ 00109 uint32_t * val_angular_velocity_covariancei = (uint32_t*) &(this->angular_velocity_covariance[i]); 00110 offset += 3; 00111 *val_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00112 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00113 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00114 *val_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00115 uint32_t exp_angular_velocity_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00116 exp_angular_velocity_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00117 if(exp_angular_velocity_covariancei !=0) 00118 *val_angular_velocity_covariancei |= ((exp_angular_velocity_covariancei)-1023+127)<<23; 00119 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->angular_velocity_covariance[i] = -this->angular_velocity_covariance[i]; 00120 } 00121 offset += this->linear_acceleration.deserialize(inbuffer + offset); 00122 uint8_t * linear_acceleration_covariance_val = (uint8_t*) this->linear_acceleration_covariance; 00123 for( uint8_t i = 0; i < 9; i++){ 00124 uint32_t * val_linear_acceleration_covariancei = (uint32_t*) &(this->linear_acceleration_covariance[i]); 00125 offset += 3; 00126 *val_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00127 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00128 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00129 *val_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00130 uint32_t exp_linear_acceleration_covariancei = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00131 exp_linear_acceleration_covariancei |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00132 if(exp_linear_acceleration_covariancei !=0) 00133 *val_linear_acceleration_covariancei |= ((exp_linear_acceleration_covariancei)-1023+127)<<23; 00134 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->linear_acceleration_covariance[i] = -this->linear_acceleration_covariance[i]; 00135 } 00136 return offset; 00137 } 00138 00139 virtual const char * getType(){ return "sensor_msgs/Imu"; }; 00140 virtual const char * getMD5(){ return "6a62c6daae103f4ff57a132d6f95cec2"; }; 00141 00142 }; 00143 00144 } 00145 #endif
Generated on Tue Jul 12 2022 19:53:57 by
1.7.2