ContactInformation.h
Go to the documentation of this file.
00001 #ifndef _ROS_moveit_msgs_ContactInformation_h
00002 #define _ROS_moveit_msgs_ContactInformation_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/Point.h"
00010 #include "geometry_msgs/Vector3.h"
00011 
00012 namespace moveit_msgs
00013 {
00014 
00015   class ContactInformation : public ros::Msg
00016   {
00017     public:
00018       std_msgs::Header header;
00019       geometry_msgs::Point position;
00020       geometry_msgs::Vector3 normal;
00021       float depth;
00022       const char* contact_body_1;
00023       uint32_t body_type_1;
00024       const char* contact_body_2;
00025       uint32_t body_type_2;
00026       enum { ROBOT_LINK = 0 };
00027       enum { WORLD_OBJECT = 1 };
00028       enum { ROBOT_ATTACHED = 2 };
00029 
00030     virtual int serialize(unsigned char *outbuffer) const
00031     {
00032       int offset = 0;
00033       offset += this->header.serialize(outbuffer + offset);
00034       offset += this->position.serialize(outbuffer + offset);
00035       offset += this->normal.serialize(outbuffer + offset);
00036       int32_t * val_depth = (int32_t *) &(this->depth);
00037       int32_t exp_depth = (((*val_depth)>>23)&255);
00038       if(exp_depth != 0)
00039         exp_depth += 1023-127;
00040       int32_t sig_depth = *val_depth;
00041       *(outbuffer + offset++) = 0;
00042       *(outbuffer + offset++) = 0;
00043       *(outbuffer + offset++) = 0;
00044       *(outbuffer + offset++) = (sig_depth<<5) & 0xff;
00045       *(outbuffer + offset++) = (sig_depth>>3) & 0xff;
00046       *(outbuffer + offset++) = (sig_depth>>11) & 0xff;
00047       *(outbuffer + offset++) = ((exp_depth<<4) & 0xF0) | ((sig_depth>>19)&0x0F);
00048       *(outbuffer + offset++) = (exp_depth>>4) & 0x7F;
00049       if(this->depth < 0) *(outbuffer + offset -1) |= 0x80;
00050       uint32_t length_contact_body_1 = strlen(this->contact_body_1);
00051       memcpy(outbuffer + offset, &length_contact_body_1, sizeof(uint32_t));
00052       offset += 4;
00053       memcpy(outbuffer + offset, this->contact_body_1, length_contact_body_1);
00054       offset += length_contact_body_1;
00055       *(outbuffer + offset + 0) = (this->body_type_1 >> (8 * 0)) & 0xFF;
00056       *(outbuffer + offset + 1) = (this->body_type_1 >> (8 * 1)) & 0xFF;
00057       *(outbuffer + offset + 2) = (this->body_type_1 >> (8 * 2)) & 0xFF;
00058       *(outbuffer + offset + 3) = (this->body_type_1 >> (8 * 3)) & 0xFF;
00059       offset += sizeof(this->body_type_1);
00060       uint32_t length_contact_body_2 = strlen(this->contact_body_2);
00061       memcpy(outbuffer + offset, &length_contact_body_2, sizeof(uint32_t));
00062       offset += 4;
00063       memcpy(outbuffer + offset, this->contact_body_2, length_contact_body_2);
00064       offset += length_contact_body_2;
00065       *(outbuffer + offset + 0) = (this->body_type_2 >> (8 * 0)) & 0xFF;
00066       *(outbuffer + offset + 1) = (this->body_type_2 >> (8 * 1)) & 0xFF;
00067       *(outbuffer + offset + 2) = (this->body_type_2 >> (8 * 2)) & 0xFF;
00068       *(outbuffer + offset + 3) = (this->body_type_2 >> (8 * 3)) & 0xFF;
00069       offset += sizeof(this->body_type_2);
00070       return offset;
00071     }
00072 
00073     virtual int deserialize(unsigned char *inbuffer)
00074     {
00075       int offset = 0;
00076       offset += this->header.deserialize(inbuffer + offset);
00077       offset += this->position.deserialize(inbuffer + offset);
00078       offset += this->normal.deserialize(inbuffer + offset);
00079       uint32_t * val_depth = (uint32_t*) &(this->depth);
00080       offset += 3;
00081       *val_depth = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00082       *val_depth |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00083       *val_depth |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00084       *val_depth |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00085       uint32_t exp_depth = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00086       exp_depth |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00087       if(exp_depth !=0)
00088         *val_depth |= ((exp_depth)-1023+127)<<23;
00089       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->depth = -this->depth;
00090       uint32_t length_contact_body_1;
00091       memcpy(&length_contact_body_1, (inbuffer + offset), sizeof(uint32_t));
00092       offset += 4;
00093       for(unsigned int k= offset; k< offset+length_contact_body_1; ++k){
00094           inbuffer[k-1]=inbuffer[k];
00095       }
00096       inbuffer[offset+length_contact_body_1-1]=0;
00097       this->contact_body_1 = (char *)(inbuffer + offset-1);
00098       offset += length_contact_body_1;
00099       this->body_type_1 =  ((uint32_t) (*(inbuffer + offset)));
00100       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00101       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00102       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00103       offset += sizeof(this->body_type_1);
00104       uint32_t length_contact_body_2;
00105       memcpy(&length_contact_body_2, (inbuffer + offset), sizeof(uint32_t));
00106       offset += 4;
00107       for(unsigned int k= offset; k< offset+length_contact_body_2; ++k){
00108           inbuffer[k-1]=inbuffer[k];
00109       }
00110       inbuffer[offset+length_contact_body_2-1]=0;
00111       this->contact_body_2 = (char *)(inbuffer + offset-1);
00112       offset += length_contact_body_2;
00113       this->body_type_2 =  ((uint32_t) (*(inbuffer + offset)));
00114       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00115       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00116       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00117       offset += sizeof(this->body_type_2);
00118      return offset;
00119     }
00120 
00121     const char * getType(){ return "moveit_msgs/ContactInformation"; };
00122     const char * getMD5(){ return "116228ca08b0c286ec5ca32a50fdc17b"; };
00123 
00124   };
00125 
00126 }
00127 #endif


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