ContactState.h
Go to the documentation of this file.
00001 #ifndef _ROS_gazebo_msgs_ContactState_h
00002 #define _ROS_gazebo_msgs_ContactState_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "geometry_msgs/Wrench.h"
00009 #include "geometry_msgs/Vector3.h"
00010 
00011 namespace gazebo_msgs
00012 {
00013 
00014   class ContactState : public ros::Msg
00015   {
00016     public:
00017       const char* info;
00018       const char* collision1_name;
00019       const char* collision2_name;
00020       uint8_t wrenches_length;
00021       geometry_msgs::Wrench st_wrenches;
00022       geometry_msgs::Wrench * wrenches;
00023       geometry_msgs::Wrench total_wrench;
00024       uint8_t contact_positions_length;
00025       geometry_msgs::Vector3 st_contact_positions;
00026       geometry_msgs::Vector3 * contact_positions;
00027       uint8_t contact_normals_length;
00028       geometry_msgs::Vector3 st_contact_normals;
00029       geometry_msgs::Vector3 * contact_normals;
00030       uint8_t depths_length;
00031       float st_depths;
00032       float * depths;
00033 
00034     virtual int serialize(unsigned char *outbuffer) const
00035     {
00036       int offset = 0;
00037       uint32_t length_info = strlen(this->info);
00038       memcpy(outbuffer + offset, &length_info, sizeof(uint32_t));
00039       offset += 4;
00040       memcpy(outbuffer + offset, this->info, length_info);
00041       offset += length_info;
00042       uint32_t length_collision1_name = strlen(this->collision1_name);
00043       memcpy(outbuffer + offset, &length_collision1_name, sizeof(uint32_t));
00044       offset += 4;
00045       memcpy(outbuffer + offset, this->collision1_name, length_collision1_name);
00046       offset += length_collision1_name;
00047       uint32_t length_collision2_name = strlen(this->collision2_name);
00048       memcpy(outbuffer + offset, &length_collision2_name, sizeof(uint32_t));
00049       offset += 4;
00050       memcpy(outbuffer + offset, this->collision2_name, length_collision2_name);
00051       offset += length_collision2_name;
00052       *(outbuffer + offset++) = wrenches_length;
00053       *(outbuffer + offset++) = 0;
00054       *(outbuffer + offset++) = 0;
00055       *(outbuffer + offset++) = 0;
00056       for( uint8_t i = 0; i < wrenches_length; i++){
00057       offset += this->wrenches[i].serialize(outbuffer + offset);
00058       }
00059       offset += this->total_wrench.serialize(outbuffer + offset);
00060       *(outbuffer + offset++) = contact_positions_length;
00061       *(outbuffer + offset++) = 0;
00062       *(outbuffer + offset++) = 0;
00063       *(outbuffer + offset++) = 0;
00064       for( uint8_t i = 0; i < contact_positions_length; i++){
00065       offset += this->contact_positions[i].serialize(outbuffer + offset);
00066       }
00067       *(outbuffer + offset++) = contact_normals_length;
00068       *(outbuffer + offset++) = 0;
00069       *(outbuffer + offset++) = 0;
00070       *(outbuffer + offset++) = 0;
00071       for( uint8_t i = 0; i < contact_normals_length; i++){
00072       offset += this->contact_normals[i].serialize(outbuffer + offset);
00073       }
00074       *(outbuffer + offset++) = depths_length;
00075       *(outbuffer + offset++) = 0;
00076       *(outbuffer + offset++) = 0;
00077       *(outbuffer + offset++) = 0;
00078       for( uint8_t i = 0; i < depths_length; i++){
00079       int32_t * val_depthsi = (int32_t *) &(this->depths[i]);
00080       int32_t exp_depthsi = (((*val_depthsi)>>23)&255);
00081       if(exp_depthsi != 0)
00082         exp_depthsi += 1023-127;
00083       int32_t sig_depthsi = *val_depthsi;
00084       *(outbuffer + offset++) = 0;
00085       *(outbuffer + offset++) = 0;
00086       *(outbuffer + offset++) = 0;
00087       *(outbuffer + offset++) = (sig_depthsi<<5) & 0xff;
00088       *(outbuffer + offset++) = (sig_depthsi>>3) & 0xff;
00089       *(outbuffer + offset++) = (sig_depthsi>>11) & 0xff;
00090       *(outbuffer + offset++) = ((exp_depthsi<<4) & 0xF0) | ((sig_depthsi>>19)&0x0F);
00091       *(outbuffer + offset++) = (exp_depthsi>>4) & 0x7F;
00092       if(this->depths[i] < 0) *(outbuffer + offset -1) |= 0x80;
00093       }
00094       return offset;
00095     }
00096 
00097     virtual int deserialize(unsigned char *inbuffer)
00098     {
00099       int offset = 0;
00100       uint32_t length_info;
00101       memcpy(&length_info, (inbuffer + offset), sizeof(uint32_t));
00102       offset += 4;
00103       for(unsigned int k= offset; k< offset+length_info; ++k){
00104           inbuffer[k-1]=inbuffer[k];
00105       }
00106       inbuffer[offset+length_info-1]=0;
00107       this->info = (char *)(inbuffer + offset-1);
00108       offset += length_info;
00109       uint32_t length_collision1_name;
00110       memcpy(&length_collision1_name, (inbuffer + offset), sizeof(uint32_t));
00111       offset += 4;
00112       for(unsigned int k= offset; k< offset+length_collision1_name; ++k){
00113           inbuffer[k-1]=inbuffer[k];
00114       }
00115       inbuffer[offset+length_collision1_name-1]=0;
00116       this->collision1_name = (char *)(inbuffer + offset-1);
00117       offset += length_collision1_name;
00118       uint32_t length_collision2_name;
00119       memcpy(&length_collision2_name, (inbuffer + offset), sizeof(uint32_t));
00120       offset += 4;
00121       for(unsigned int k= offset; k< offset+length_collision2_name; ++k){
00122           inbuffer[k-1]=inbuffer[k];
00123       }
00124       inbuffer[offset+length_collision2_name-1]=0;
00125       this->collision2_name = (char *)(inbuffer + offset-1);
00126       offset += length_collision2_name;
00127       uint8_t wrenches_lengthT = *(inbuffer + offset++);
00128       if(wrenches_lengthT > wrenches_length)
00129         this->wrenches = (geometry_msgs::Wrench*)realloc(this->wrenches, wrenches_lengthT * sizeof(geometry_msgs::Wrench));
00130       offset += 3;
00131       wrenches_length = wrenches_lengthT;
00132       for( uint8_t i = 0; i < wrenches_length; i++){
00133       offset += this->st_wrenches.deserialize(inbuffer + offset);
00134         memcpy( &(this->wrenches[i]), &(this->st_wrenches), sizeof(geometry_msgs::Wrench));
00135       }
00136       offset += this->total_wrench.deserialize(inbuffer + offset);
00137       uint8_t contact_positions_lengthT = *(inbuffer + offset++);
00138       if(contact_positions_lengthT > contact_positions_length)
00139         this->contact_positions = (geometry_msgs::Vector3*)realloc(this->contact_positions, contact_positions_lengthT * sizeof(geometry_msgs::Vector3));
00140       offset += 3;
00141       contact_positions_length = contact_positions_lengthT;
00142       for( uint8_t i = 0; i < contact_positions_length; i++){
00143       offset += this->st_contact_positions.deserialize(inbuffer + offset);
00144         memcpy( &(this->contact_positions[i]), &(this->st_contact_positions), sizeof(geometry_msgs::Vector3));
00145       }
00146       uint8_t contact_normals_lengthT = *(inbuffer + offset++);
00147       if(contact_normals_lengthT > contact_normals_length)
00148         this->contact_normals = (geometry_msgs::Vector3*)realloc(this->contact_normals, contact_normals_lengthT * sizeof(geometry_msgs::Vector3));
00149       offset += 3;
00150       contact_normals_length = contact_normals_lengthT;
00151       for( uint8_t i = 0; i < contact_normals_length; i++){
00152       offset += this->st_contact_normals.deserialize(inbuffer + offset);
00153         memcpy( &(this->contact_normals[i]), &(this->st_contact_normals), sizeof(geometry_msgs::Vector3));
00154       }
00155       uint8_t depths_lengthT = *(inbuffer + offset++);
00156       if(depths_lengthT > depths_length)
00157         this->depths = (float*)realloc(this->depths, depths_lengthT * sizeof(float));
00158       offset += 3;
00159       depths_length = depths_lengthT;
00160       for( uint8_t i = 0; i < depths_length; i++){
00161       uint32_t * val_st_depths = (uint32_t*) &(this->st_depths);
00162       offset += 3;
00163       *val_st_depths = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00164       *val_st_depths |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00165       *val_st_depths |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00166       *val_st_depths |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00167       uint32_t exp_st_depths = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00168       exp_st_depths |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00169       if(exp_st_depths !=0)
00170         *val_st_depths |= ((exp_st_depths)-1023+127)<<23;
00171       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_depths = -this->st_depths;
00172         memcpy( &(this->depths[i]), &(this->st_depths), sizeof(float));
00173       }
00174      return offset;
00175     }
00176 
00177     const char * getType(){ return "gazebo_msgs/ContactState"; };
00178     const char * getMD5(){ return "48c0ffb054b8c444f870cecea1ee50d9"; };
00179 
00180   };
00181 
00182 }
00183 #endif


ric_mc
Author(s): RoboTiCan
autogenerated on Fri May 20 2016 03:48:55