ContactInformation.h

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       typedef std_msgs::Header _header_type;
00019       _header_type header;
00020       typedef geometry_msgs::Point _position_type;
00021       _position_type position;
00022       typedef geometry_msgs::Vector3 _normal_type;
00023       _normal_type normal;
00024       typedef double _depth_type;
00025       _depth_type depth;
00026       typedef const char* _contact_body_1_type;
00027       _contact_body_1_type contact_body_1;
00028       typedef uint32_t _body_type_1_type;
00029       _body_type_1_type body_type_1;
00030       typedef const char* _contact_body_2_type;
00031       _contact_body_2_type contact_body_2;
00032       typedef uint32_t _body_type_2_type;
00033       _body_type_2_type body_type_2;
00034       enum { ROBOT_LINK = 0 };
00035       enum { WORLD_OBJECT = 1 };
00036       enum { ROBOT_ATTACHED = 2 };
00037 
00038     ContactInformation():
00039       header(),
00040       position(),
00041       normal(),
00042       depth(0),
00043       contact_body_1(""),
00044       body_type_1(0),
00045       contact_body_2(""),
00046       body_type_2(0)
00047     {
00048     }
00049 
00050     virtual int serialize(unsigned char *outbuffer) const
00051     {
00052       int offset = 0;
00053       offset += this->header.serialize(outbuffer + offset);
00054       offset += this->position.serialize(outbuffer + offset);
00055       offset += this->normal.serialize(outbuffer + offset);
00056       union {
00057         double real;
00058         uint64_t base;
00059       } u_depth;
00060       u_depth.real = this->depth;
00061       *(outbuffer + offset + 0) = (u_depth.base >> (8 * 0)) & 0xFF;
00062       *(outbuffer + offset + 1) = (u_depth.base >> (8 * 1)) & 0xFF;
00063       *(outbuffer + offset + 2) = (u_depth.base >> (8 * 2)) & 0xFF;
00064       *(outbuffer + offset + 3) = (u_depth.base >> (8 * 3)) & 0xFF;
00065       *(outbuffer + offset + 4) = (u_depth.base >> (8 * 4)) & 0xFF;
00066       *(outbuffer + offset + 5) = (u_depth.base >> (8 * 5)) & 0xFF;
00067       *(outbuffer + offset + 6) = (u_depth.base >> (8 * 6)) & 0xFF;
00068       *(outbuffer + offset + 7) = (u_depth.base >> (8 * 7)) & 0xFF;
00069       offset += sizeof(this->depth);
00070       uint32_t length_contact_body_1 = strlen(this->contact_body_1);
00071       varToArr(outbuffer + offset, length_contact_body_1);
00072       offset += 4;
00073       memcpy(outbuffer + offset, this->contact_body_1, length_contact_body_1);
00074       offset += length_contact_body_1;
00075       *(outbuffer + offset + 0) = (this->body_type_1 >> (8 * 0)) & 0xFF;
00076       *(outbuffer + offset + 1) = (this->body_type_1 >> (8 * 1)) & 0xFF;
00077       *(outbuffer + offset + 2) = (this->body_type_1 >> (8 * 2)) & 0xFF;
00078       *(outbuffer + offset + 3) = (this->body_type_1 >> (8 * 3)) & 0xFF;
00079       offset += sizeof(this->body_type_1);
00080       uint32_t length_contact_body_2 = strlen(this->contact_body_2);
00081       varToArr(outbuffer + offset, length_contact_body_2);
00082       offset += 4;
00083       memcpy(outbuffer + offset, this->contact_body_2, length_contact_body_2);
00084       offset += length_contact_body_2;
00085       *(outbuffer + offset + 0) = (this->body_type_2 >> (8 * 0)) & 0xFF;
00086       *(outbuffer + offset + 1) = (this->body_type_2 >> (8 * 1)) & 0xFF;
00087       *(outbuffer + offset + 2) = (this->body_type_2 >> (8 * 2)) & 0xFF;
00088       *(outbuffer + offset + 3) = (this->body_type_2 >> (8 * 3)) & 0xFF;
00089       offset += sizeof(this->body_type_2);
00090       return offset;
00091     }
00092 
00093     virtual int deserialize(unsigned char *inbuffer)
00094     {
00095       int offset = 0;
00096       offset += this->header.deserialize(inbuffer + offset);
00097       offset += this->position.deserialize(inbuffer + offset);
00098       offset += this->normal.deserialize(inbuffer + offset);
00099       union {
00100         double real;
00101         uint64_t base;
00102       } u_depth;
00103       u_depth.base = 0;
00104       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0);
00105       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1);
00106       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2);
00107       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3);
00108       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4);
00109       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5);
00110       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6);
00111       u_depth.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7);
00112       this->depth = u_depth.real;
00113       offset += sizeof(this->depth);
00114       uint32_t length_contact_body_1;
00115       arrToVar(length_contact_body_1, (inbuffer + offset));
00116       offset += 4;
00117       for(unsigned int k= offset; k< offset+length_contact_body_1; ++k){
00118           inbuffer[k-1]=inbuffer[k];
00119       }
00120       inbuffer[offset+length_contact_body_1-1]=0;
00121       this->contact_body_1 = (char *)(inbuffer + offset-1);
00122       offset += length_contact_body_1;
00123       this->body_type_1 =  ((uint32_t) (*(inbuffer + offset)));
00124       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00125       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00126       this->body_type_1 |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00127       offset += sizeof(this->body_type_1);
00128       uint32_t length_contact_body_2;
00129       arrToVar(length_contact_body_2, (inbuffer + offset));
00130       offset += 4;
00131       for(unsigned int k= offset; k< offset+length_contact_body_2; ++k){
00132           inbuffer[k-1]=inbuffer[k];
00133       }
00134       inbuffer[offset+length_contact_body_2-1]=0;
00135       this->contact_body_2 = (char *)(inbuffer + offset-1);
00136       offset += length_contact_body_2;
00137       this->body_type_2 =  ((uint32_t) (*(inbuffer + offset)));
00138       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00139       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00140       this->body_type_2 |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00141       offset += sizeof(this->body_type_2);
00142      return offset;
00143     }
00144 
00145     virtual const char * getType(){ return "moveit_msgs/ContactInformation"; };
00146     virtual const char * getMD5(){ return "116228ca08b0c286ec5ca32a50fdc17b"; };
00147 
00148   };
00149 
00150 }
00151 #endif