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