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
JointState.h
00001 #ifndef _ROS_sensor_msgs_JointState_h 00002 #define _ROS_sensor_msgs_JointState_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 00010 namespace sensor_msgs 00011 { 00012 00013 class JointState : public ros::Msg 00014 { 00015 public: 00016 std_msgs::Header header; 00017 uint8_t name_length; 00018 char* st_name; 00019 char* * name; 00020 uint8_t position_length; 00021 float st_position; 00022 float * position; 00023 uint8_t velocity_length; 00024 float st_velocity; 00025 float * velocity; 00026 uint8_t effort_length; 00027 float st_effort; 00028 float * effort; 00029 00030 virtual int serialize(unsigned char *outbuffer) const 00031 { 00032 int offset = 0; 00033 offset += this->header.serialize(outbuffer + offset); 00034 *(outbuffer + offset++) = name_length; 00035 *(outbuffer + offset++) = 0; 00036 *(outbuffer + offset++) = 0; 00037 *(outbuffer + offset++) = 0; 00038 for( uint8_t i = 0; i < name_length; i++){ 00039 uint32_t * length_namei = (uint32_t *)(outbuffer + offset); 00040 *length_namei = strlen( (const char*) this->name[i]); 00041 offset += 4; 00042 memcpy(outbuffer + offset, this->name[i], *length_namei); 00043 offset += *length_namei; 00044 } 00045 *(outbuffer + offset++) = position_length; 00046 *(outbuffer + offset++) = 0; 00047 *(outbuffer + offset++) = 0; 00048 *(outbuffer + offset++) = 0; 00049 for( uint8_t i = 0; i < position_length; i++){ 00050 int32_t * val_positioni = (long *) &(this->position[i]); 00051 int32_t exp_positioni = (((*val_positioni)>>23)&255); 00052 if(exp_positioni != 0) 00053 exp_positioni += 1023-127; 00054 int32_t sig_positioni = *val_positioni; 00055 *(outbuffer + offset++) = 0; 00056 *(outbuffer + offset++) = 0; 00057 *(outbuffer + offset++) = 0; 00058 *(outbuffer + offset++) = (sig_positioni<<5) & 0xff; 00059 *(outbuffer + offset++) = (sig_positioni>>3) & 0xff; 00060 *(outbuffer + offset++) = (sig_positioni>>11) & 0xff; 00061 *(outbuffer + offset++) = ((exp_positioni<<4) & 0xF0) | ((sig_positioni>>19)&0x0F); 00062 *(outbuffer + offset++) = (exp_positioni>>4) & 0x7F; 00063 if(this->position[i] < 0) *(outbuffer + offset -1) |= 0x80; 00064 } 00065 *(outbuffer + offset++) = velocity_length; 00066 *(outbuffer + offset++) = 0; 00067 *(outbuffer + offset++) = 0; 00068 *(outbuffer + offset++) = 0; 00069 for( uint8_t i = 0; i < velocity_length; i++){ 00070 int32_t * val_velocityi = (long *) &(this->velocity[i]); 00071 int32_t exp_velocityi = (((*val_velocityi)>>23)&255); 00072 if(exp_velocityi != 0) 00073 exp_velocityi += 1023-127; 00074 int32_t sig_velocityi = *val_velocityi; 00075 *(outbuffer + offset++) = 0; 00076 *(outbuffer + offset++) = 0; 00077 *(outbuffer + offset++) = 0; 00078 *(outbuffer + offset++) = (sig_velocityi<<5) & 0xff; 00079 *(outbuffer + offset++) = (sig_velocityi>>3) & 0xff; 00080 *(outbuffer + offset++) = (sig_velocityi>>11) & 0xff; 00081 *(outbuffer + offset++) = ((exp_velocityi<<4) & 0xF0) | ((sig_velocityi>>19)&0x0F); 00082 *(outbuffer + offset++) = (exp_velocityi>>4) & 0x7F; 00083 if(this->velocity[i] < 0) *(outbuffer + offset -1) |= 0x80; 00084 } 00085 *(outbuffer + offset++) = effort_length; 00086 *(outbuffer + offset++) = 0; 00087 *(outbuffer + offset++) = 0; 00088 *(outbuffer + offset++) = 0; 00089 for( uint8_t i = 0; i < effort_length; i++){ 00090 int32_t * val_efforti = (long *) &(this->effort[i]); 00091 int32_t exp_efforti = (((*val_efforti)>>23)&255); 00092 if(exp_efforti != 0) 00093 exp_efforti += 1023-127; 00094 int32_t sig_efforti = *val_efforti; 00095 *(outbuffer + offset++) = 0; 00096 *(outbuffer + offset++) = 0; 00097 *(outbuffer + offset++) = 0; 00098 *(outbuffer + offset++) = (sig_efforti<<5) & 0xff; 00099 *(outbuffer + offset++) = (sig_efforti>>3) & 0xff; 00100 *(outbuffer + offset++) = (sig_efforti>>11) & 0xff; 00101 *(outbuffer + offset++) = ((exp_efforti<<4) & 0xF0) | ((sig_efforti>>19)&0x0F); 00102 *(outbuffer + offset++) = (exp_efforti>>4) & 0x7F; 00103 if(this->effort[i] < 0) *(outbuffer + offset -1) |= 0x80; 00104 } 00105 return offset; 00106 } 00107 00108 virtual int deserialize(unsigned char *inbuffer) 00109 { 00110 int offset = 0; 00111 offset += this->header.deserialize(inbuffer + offset); 00112 uint8_t name_lengthT = *(inbuffer + offset++); 00113 if(name_lengthT > name_length) 00114 this->name = (char**)realloc(this->name, name_lengthT * sizeof(char*)); 00115 offset += 3; 00116 name_length = name_lengthT; 00117 for( uint8_t i = 0; i < name_length; i++){ 00118 uint32_t length_st_name = *(uint32_t *)(inbuffer + offset); 00119 offset += 4; 00120 for(unsigned int k= offset; k< offset+length_st_name; ++k){ 00121 inbuffer[k-1]=inbuffer[k]; 00122 } 00123 inbuffer[offset+length_st_name-1]=0; 00124 this->st_name = (char *)(inbuffer + offset-1); 00125 offset += length_st_name; 00126 memcpy( &(this->name[i]), &(this->st_name), sizeof(char*)); 00127 } 00128 uint8_t position_lengthT = *(inbuffer + offset++); 00129 if(position_lengthT > position_length) 00130 this->position = (float*)realloc(this->position, position_lengthT * sizeof(float)); 00131 offset += 3; 00132 position_length = position_lengthT; 00133 for( uint8_t i = 0; i < position_length; i++){ 00134 uint32_t * val_st_position = (uint32_t*) &(this->st_position); 00135 offset += 3; 00136 *val_st_position = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00137 *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00138 *val_st_position |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00139 *val_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00140 uint32_t exp_st_position = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00141 exp_st_position |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00142 if(exp_st_position !=0) 00143 *val_st_position |= ((exp_st_position)-1023+127)<<23; 00144 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_position = -this->st_position; 00145 memcpy( &(this->position[i]), &(this->st_position), sizeof(float)); 00146 } 00147 uint8_t velocity_lengthT = *(inbuffer + offset++); 00148 if(velocity_lengthT > velocity_length) 00149 this->velocity = (float*)realloc(this->velocity, velocity_lengthT * sizeof(float)); 00150 offset += 3; 00151 velocity_length = velocity_lengthT; 00152 for( uint8_t i = 0; i < velocity_length; i++){ 00153 uint32_t * val_st_velocity = (uint32_t*) &(this->st_velocity); 00154 offset += 3; 00155 *val_st_velocity = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00156 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00157 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00158 *val_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00159 uint32_t exp_st_velocity = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00160 exp_st_velocity |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00161 if(exp_st_velocity !=0) 00162 *val_st_velocity |= ((exp_st_velocity)-1023+127)<<23; 00163 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_velocity = -this->st_velocity; 00164 memcpy( &(this->velocity[i]), &(this->st_velocity), sizeof(float)); 00165 } 00166 uint8_t effort_lengthT = *(inbuffer + offset++); 00167 if(effort_lengthT > effort_length) 00168 this->effort = (float*)realloc(this->effort, effort_lengthT * sizeof(float)); 00169 offset += 3; 00170 effort_length = effort_lengthT; 00171 for( uint8_t i = 0; i < effort_length; i++){ 00172 uint32_t * val_st_effort = (uint32_t*) &(this->st_effort); 00173 offset += 3; 00174 *val_st_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); 00175 *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; 00176 *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; 00177 *val_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; 00178 uint32_t exp_st_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; 00179 exp_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; 00180 if(exp_st_effort !=0) 00181 *val_st_effort |= ((exp_st_effort)-1023+127)<<23; 00182 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_effort = -this->st_effort; 00183 memcpy( &(this->effort[i]), &(this->st_effort), sizeof(float)); 00184 } 00185 return offset; 00186 } 00187 00188 virtual const char * getType(){ return "sensor_msgs/JointState"; }; 00189 virtual const char * getMD5(){ return "3066dcd76a6cfaef579bd0f34173e9fd"; }; 00190 00191 }; 00192 00193 } 00194 #endif
Generated on Tue Jul 12 2022 19:53:57 by
1.7.2