Go to the documentation of this file.00001 #ifndef _ROS_moveit_msgs_AttachedCollisionObject_h
00002 #define _ROS_moveit_msgs_AttachedCollisionObject_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "moveit_msgs/CollisionObject.h"
00009 #include "trajectory_msgs/JointTrajectory.h"
00010
00011 namespace moveit_msgs
00012 {
00013
00014 class AttachedCollisionObject : public ros::Msg
00015 {
00016 public:
00017 const char* link_name;
00018 moveit_msgs::CollisionObject object;
00019 uint8_t touch_links_length;
00020 char* st_touch_links;
00021 char* * touch_links;
00022 trajectory_msgs::JointTrajectory detach_posture;
00023 float weight;
00024
00025 virtual int serialize(unsigned char *outbuffer) const
00026 {
00027 int offset = 0;
00028 uint32_t length_link_name = strlen(this->link_name);
00029 memcpy(outbuffer + offset, &length_link_name, sizeof(uint32_t));
00030 offset += 4;
00031 memcpy(outbuffer + offset, this->link_name, length_link_name);
00032 offset += length_link_name;
00033 offset += this->object.serialize(outbuffer + offset);
00034 *(outbuffer + offset++) = touch_links_length;
00035 *(outbuffer + offset++) = 0;
00036 *(outbuffer + offset++) = 0;
00037 *(outbuffer + offset++) = 0;
00038 for( uint8_t i = 0; i < touch_links_length; i++){
00039 uint32_t length_touch_linksi = strlen(this->touch_links[i]);
00040 memcpy(outbuffer + offset, &length_touch_linksi, sizeof(uint32_t));
00041 offset += 4;
00042 memcpy(outbuffer + offset, this->touch_links[i], length_touch_linksi);
00043 offset += length_touch_linksi;
00044 }
00045 offset += this->detach_posture.serialize(outbuffer + offset);
00046 int32_t * val_weight = (int32_t *) &(this->weight);
00047 int32_t exp_weight = (((*val_weight)>>23)&255);
00048 if(exp_weight != 0)
00049 exp_weight += 1023-127;
00050 int32_t sig_weight = *val_weight;
00051 *(outbuffer + offset++) = 0;
00052 *(outbuffer + offset++) = 0;
00053 *(outbuffer + offset++) = 0;
00054 *(outbuffer + offset++) = (sig_weight<<5) & 0xff;
00055 *(outbuffer + offset++) = (sig_weight>>3) & 0xff;
00056 *(outbuffer + offset++) = (sig_weight>>11) & 0xff;
00057 *(outbuffer + offset++) = ((exp_weight<<4) & 0xF0) | ((sig_weight>>19)&0x0F);
00058 *(outbuffer + offset++) = (exp_weight>>4) & 0x7F;
00059 if(this->weight < 0) *(outbuffer + offset -1) |= 0x80;
00060 return offset;
00061 }
00062
00063 virtual int deserialize(unsigned char *inbuffer)
00064 {
00065 int offset = 0;
00066 uint32_t length_link_name;
00067 memcpy(&length_link_name, (inbuffer + offset), sizeof(uint32_t));
00068 offset += 4;
00069 for(unsigned int k= offset; k< offset+length_link_name; ++k){
00070 inbuffer[k-1]=inbuffer[k];
00071 }
00072 inbuffer[offset+length_link_name-1]=0;
00073 this->link_name = (char *)(inbuffer + offset-1);
00074 offset += length_link_name;
00075 offset += this->object.deserialize(inbuffer + offset);
00076 uint8_t touch_links_lengthT = *(inbuffer + offset++);
00077 if(touch_links_lengthT > touch_links_length)
00078 this->touch_links = (char**)realloc(this->touch_links, touch_links_lengthT * sizeof(char*));
00079 offset += 3;
00080 touch_links_length = touch_links_lengthT;
00081 for( uint8_t i = 0; i < touch_links_length; i++){
00082 uint32_t length_st_touch_links;
00083 memcpy(&length_st_touch_links, (inbuffer + offset), sizeof(uint32_t));
00084 offset += 4;
00085 for(unsigned int k= offset; k< offset+length_st_touch_links; ++k){
00086 inbuffer[k-1]=inbuffer[k];
00087 }
00088 inbuffer[offset+length_st_touch_links-1]=0;
00089 this->st_touch_links = (char *)(inbuffer + offset-1);
00090 offset += length_st_touch_links;
00091 memcpy( &(this->touch_links[i]), &(this->st_touch_links), sizeof(char*));
00092 }
00093 offset += this->detach_posture.deserialize(inbuffer + offset);
00094 uint32_t * val_weight = (uint32_t*) &(this->weight);
00095 offset += 3;
00096 *val_weight = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00097 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00098 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00099 *val_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00100 uint32_t exp_weight = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00101 exp_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00102 if(exp_weight !=0)
00103 *val_weight |= ((exp_weight)-1023+127)<<23;
00104 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->weight = -this->weight;
00105 return offset;
00106 }
00107
00108 const char * getType(){ return "moveit_msgs/AttachedCollisionObject"; };
00109 const char * getMD5(){ return "3ceac60b21e85bbd6c5b0ab9d476b752"; };
00110
00111 };
00112
00113 }
00114 #endif