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