Go to the documentation of this file.00001 #ifndef _ROS_moveit_msgs_OrientationConstraint_h
00002 #define _ROS_moveit_msgs_OrientationConstraint_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/Quaternion.h"
00010
00011 namespace moveit_msgs
00012 {
00013
00014 class OrientationConstraint : public ros::Msg
00015 {
00016 public:
00017 std_msgs::Header header;
00018 geometry_msgs::Quaternion orientation;
00019 const char* link_name;
00020 float absolute_x_axis_tolerance;
00021 float absolute_y_axis_tolerance;
00022 float absolute_z_axis_tolerance;
00023 float weight;
00024
00025 virtual int serialize(unsigned char *outbuffer) const
00026 {
00027 int offset = 0;
00028 offset += this->header.serialize(outbuffer + offset);
00029 offset += this->orientation.serialize(outbuffer + offset);
00030 uint32_t length_link_name = strlen(this->link_name);
00031 memcpy(outbuffer + offset, &length_link_name, sizeof(uint32_t));
00032 offset += 4;
00033 memcpy(outbuffer + offset, this->link_name, length_link_name);
00034 offset += length_link_name;
00035 int32_t * val_absolute_x_axis_tolerance = (int32_t *) &(this->absolute_x_axis_tolerance);
00036 int32_t exp_absolute_x_axis_tolerance = (((*val_absolute_x_axis_tolerance)>>23)&255);
00037 if(exp_absolute_x_axis_tolerance != 0)
00038 exp_absolute_x_axis_tolerance += 1023-127;
00039 int32_t sig_absolute_x_axis_tolerance = *val_absolute_x_axis_tolerance;
00040 *(outbuffer + offset++) = 0;
00041 *(outbuffer + offset++) = 0;
00042 *(outbuffer + offset++) = 0;
00043 *(outbuffer + offset++) = (sig_absolute_x_axis_tolerance<<5) & 0xff;
00044 *(outbuffer + offset++) = (sig_absolute_x_axis_tolerance>>3) & 0xff;
00045 *(outbuffer + offset++) = (sig_absolute_x_axis_tolerance>>11) & 0xff;
00046 *(outbuffer + offset++) = ((exp_absolute_x_axis_tolerance<<4) & 0xF0) | ((sig_absolute_x_axis_tolerance>>19)&0x0F);
00047 *(outbuffer + offset++) = (exp_absolute_x_axis_tolerance>>4) & 0x7F;
00048 if(this->absolute_x_axis_tolerance < 0) *(outbuffer + offset -1) |= 0x80;
00049 int32_t * val_absolute_y_axis_tolerance = (int32_t *) &(this->absolute_y_axis_tolerance);
00050 int32_t exp_absolute_y_axis_tolerance = (((*val_absolute_y_axis_tolerance)>>23)&255);
00051 if(exp_absolute_y_axis_tolerance != 0)
00052 exp_absolute_y_axis_tolerance += 1023-127;
00053 int32_t sig_absolute_y_axis_tolerance = *val_absolute_y_axis_tolerance;
00054 *(outbuffer + offset++) = 0;
00055 *(outbuffer + offset++) = 0;
00056 *(outbuffer + offset++) = 0;
00057 *(outbuffer + offset++) = (sig_absolute_y_axis_tolerance<<5) & 0xff;
00058 *(outbuffer + offset++) = (sig_absolute_y_axis_tolerance>>3) & 0xff;
00059 *(outbuffer + offset++) = (sig_absolute_y_axis_tolerance>>11) & 0xff;
00060 *(outbuffer + offset++) = ((exp_absolute_y_axis_tolerance<<4) & 0xF0) | ((sig_absolute_y_axis_tolerance>>19)&0x0F);
00061 *(outbuffer + offset++) = (exp_absolute_y_axis_tolerance>>4) & 0x7F;
00062 if(this->absolute_y_axis_tolerance < 0) *(outbuffer + offset -1) |= 0x80;
00063 int32_t * val_absolute_z_axis_tolerance = (int32_t *) &(this->absolute_z_axis_tolerance);
00064 int32_t exp_absolute_z_axis_tolerance = (((*val_absolute_z_axis_tolerance)>>23)&255);
00065 if(exp_absolute_z_axis_tolerance != 0)
00066 exp_absolute_z_axis_tolerance += 1023-127;
00067 int32_t sig_absolute_z_axis_tolerance = *val_absolute_z_axis_tolerance;
00068 *(outbuffer + offset++) = 0;
00069 *(outbuffer + offset++) = 0;
00070 *(outbuffer + offset++) = 0;
00071 *(outbuffer + offset++) = (sig_absolute_z_axis_tolerance<<5) & 0xff;
00072 *(outbuffer + offset++) = (sig_absolute_z_axis_tolerance>>3) & 0xff;
00073 *(outbuffer + offset++) = (sig_absolute_z_axis_tolerance>>11) & 0xff;
00074 *(outbuffer + offset++) = ((exp_absolute_z_axis_tolerance<<4) & 0xF0) | ((sig_absolute_z_axis_tolerance>>19)&0x0F);
00075 *(outbuffer + offset++) = (exp_absolute_z_axis_tolerance>>4) & 0x7F;
00076 if(this->absolute_z_axis_tolerance < 0) *(outbuffer + offset -1) |= 0x80;
00077 int32_t * val_weight = (int32_t *) &(this->weight);
00078 int32_t exp_weight = (((*val_weight)>>23)&255);
00079 if(exp_weight != 0)
00080 exp_weight += 1023-127;
00081 int32_t sig_weight = *val_weight;
00082 *(outbuffer + offset++) = 0;
00083 *(outbuffer + offset++) = 0;
00084 *(outbuffer + offset++) = 0;
00085 *(outbuffer + offset++) = (sig_weight<<5) & 0xff;
00086 *(outbuffer + offset++) = (sig_weight>>3) & 0xff;
00087 *(outbuffer + offset++) = (sig_weight>>11) & 0xff;
00088 *(outbuffer + offset++) = ((exp_weight<<4) & 0xF0) | ((sig_weight>>19)&0x0F);
00089 *(outbuffer + offset++) = (exp_weight>>4) & 0x7F;
00090 if(this->weight < 0) *(outbuffer + offset -1) |= 0x80;
00091 return offset;
00092 }
00093
00094 virtual int deserialize(unsigned char *inbuffer)
00095 {
00096 int offset = 0;
00097 offset += this->header.deserialize(inbuffer + offset);
00098 offset += this->orientation.deserialize(inbuffer + offset);
00099 uint32_t length_link_name;
00100 memcpy(&length_link_name, (inbuffer + offset), sizeof(uint32_t));
00101 offset += 4;
00102 for(unsigned int k= offset; k< offset+length_link_name; ++k){
00103 inbuffer[k-1]=inbuffer[k];
00104 }
00105 inbuffer[offset+length_link_name-1]=0;
00106 this->link_name = (char *)(inbuffer + offset-1);
00107 offset += length_link_name;
00108 uint32_t * val_absolute_x_axis_tolerance = (uint32_t*) &(this->absolute_x_axis_tolerance);
00109 offset += 3;
00110 *val_absolute_x_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00111 *val_absolute_x_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00112 *val_absolute_x_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00113 *val_absolute_x_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00114 uint32_t exp_absolute_x_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00115 exp_absolute_x_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00116 if(exp_absolute_x_axis_tolerance !=0)
00117 *val_absolute_x_axis_tolerance |= ((exp_absolute_x_axis_tolerance)-1023+127)<<23;
00118 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->absolute_x_axis_tolerance = -this->absolute_x_axis_tolerance;
00119 uint32_t * val_absolute_y_axis_tolerance = (uint32_t*) &(this->absolute_y_axis_tolerance);
00120 offset += 3;
00121 *val_absolute_y_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00122 *val_absolute_y_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00123 *val_absolute_y_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00124 *val_absolute_y_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00125 uint32_t exp_absolute_y_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00126 exp_absolute_y_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00127 if(exp_absolute_y_axis_tolerance !=0)
00128 *val_absolute_y_axis_tolerance |= ((exp_absolute_y_axis_tolerance)-1023+127)<<23;
00129 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->absolute_y_axis_tolerance = -this->absolute_y_axis_tolerance;
00130 uint32_t * val_absolute_z_axis_tolerance = (uint32_t*) &(this->absolute_z_axis_tolerance);
00131 offset += 3;
00132 *val_absolute_z_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00133 *val_absolute_z_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00134 *val_absolute_z_axis_tolerance |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00135 *val_absolute_z_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00136 uint32_t exp_absolute_z_axis_tolerance = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00137 exp_absolute_z_axis_tolerance |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00138 if(exp_absolute_z_axis_tolerance !=0)
00139 *val_absolute_z_axis_tolerance |= ((exp_absolute_z_axis_tolerance)-1023+127)<<23;
00140 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->absolute_z_axis_tolerance = -this->absolute_z_axis_tolerance;
00141 uint32_t * val_weight = (uint32_t*) &(this->weight);
00142 offset += 3;
00143 *val_weight = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00144 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00145 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00146 *val_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00147 uint32_t exp_weight = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00148 exp_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00149 if(exp_weight !=0)
00150 *val_weight |= ((exp_weight)-1023+127)<<23;
00151 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->weight = -this->weight;
00152 return offset;
00153 }
00154
00155 const char * getType(){ return "moveit_msgs/OrientationConstraint"; };
00156 const char * getMD5(){ return "ab5cefb9bc4c0089620f5eb4caf4e59a"; };
00157
00158 };
00159
00160 }
00161 #endif