Grasp.h
Go to the documentation of this file.
00001 #ifndef _ROS_manipulation_msgs_Grasp_h
00002 #define _ROS_manipulation_msgs_Grasp_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "sensor_msgs/JointState.h"
00009 #include "geometry_msgs/PoseStamped.h"
00010 #include "manipulation_msgs/GripperTranslation.h"
00011 
00012 namespace manipulation_msgs
00013 {
00014 
00015   class Grasp : public ros::Msg
00016   {
00017     public:
00018       const char* id;
00019       sensor_msgs::JointState pre_grasp_posture;
00020       sensor_msgs::JointState grasp_posture;
00021       geometry_msgs::PoseStamped grasp_pose;
00022       float grasp_quality;
00023       manipulation_msgs::GripperTranslation approach;
00024       manipulation_msgs::GripperTranslation retreat;
00025       float max_contact_force;
00026       uint8_t allowed_touch_objects_length;
00027       char* st_allowed_touch_objects;
00028       char* * allowed_touch_objects;
00029 
00030     virtual int serialize(unsigned char *outbuffer) const
00031     {
00032       int offset = 0;
00033       uint32_t length_id = strlen(this->id);
00034       memcpy(outbuffer + offset, &length_id, sizeof(uint32_t));
00035       offset += 4;
00036       memcpy(outbuffer + offset, this->id, length_id);
00037       offset += length_id;
00038       offset += this->pre_grasp_posture.serialize(outbuffer + offset);
00039       offset += this->grasp_posture.serialize(outbuffer + offset);
00040       offset += this->grasp_pose.serialize(outbuffer + offset);
00041       int32_t * val_grasp_quality = (int32_t *) &(this->grasp_quality);
00042       int32_t exp_grasp_quality = (((*val_grasp_quality)>>23)&255);
00043       if(exp_grasp_quality != 0)
00044         exp_grasp_quality += 1023-127;
00045       int32_t sig_grasp_quality = *val_grasp_quality;
00046       *(outbuffer + offset++) = 0;
00047       *(outbuffer + offset++) = 0;
00048       *(outbuffer + offset++) = 0;
00049       *(outbuffer + offset++) = (sig_grasp_quality<<5) & 0xff;
00050       *(outbuffer + offset++) = (sig_grasp_quality>>3) & 0xff;
00051       *(outbuffer + offset++) = (sig_grasp_quality>>11) & 0xff;
00052       *(outbuffer + offset++) = ((exp_grasp_quality<<4) & 0xF0) | ((sig_grasp_quality>>19)&0x0F);
00053       *(outbuffer + offset++) = (exp_grasp_quality>>4) & 0x7F;
00054       if(this->grasp_quality < 0) *(outbuffer + offset -1) |= 0x80;
00055       offset += this->approach.serialize(outbuffer + offset);
00056       offset += this->retreat.serialize(outbuffer + offset);
00057       union {
00058         float real;
00059         uint32_t base;
00060       } u_max_contact_force;
00061       u_max_contact_force.real = this->max_contact_force;
00062       *(outbuffer + offset + 0) = (u_max_contact_force.base >> (8 * 0)) & 0xFF;
00063       *(outbuffer + offset + 1) = (u_max_contact_force.base >> (8 * 1)) & 0xFF;
00064       *(outbuffer + offset + 2) = (u_max_contact_force.base >> (8 * 2)) & 0xFF;
00065       *(outbuffer + offset + 3) = (u_max_contact_force.base >> (8 * 3)) & 0xFF;
00066       offset += sizeof(this->max_contact_force);
00067       *(outbuffer + offset++) = allowed_touch_objects_length;
00068       *(outbuffer + offset++) = 0;
00069       *(outbuffer + offset++) = 0;
00070       *(outbuffer + offset++) = 0;
00071       for( uint8_t i = 0; i < allowed_touch_objects_length; i++){
00072       uint32_t length_allowed_touch_objectsi = strlen(this->allowed_touch_objects[i]);
00073       memcpy(outbuffer + offset, &length_allowed_touch_objectsi, sizeof(uint32_t));
00074       offset += 4;
00075       memcpy(outbuffer + offset, this->allowed_touch_objects[i], length_allowed_touch_objectsi);
00076       offset += length_allowed_touch_objectsi;
00077       }
00078       return offset;
00079     }
00080 
00081     virtual int deserialize(unsigned char *inbuffer)
00082     {
00083       int offset = 0;
00084       uint32_t length_id;
00085       memcpy(&length_id, (inbuffer + offset), sizeof(uint32_t));
00086       offset += 4;
00087       for(unsigned int k= offset; k< offset+length_id; ++k){
00088           inbuffer[k-1]=inbuffer[k];
00089       }
00090       inbuffer[offset+length_id-1]=0;
00091       this->id = (char *)(inbuffer + offset-1);
00092       offset += length_id;
00093       offset += this->pre_grasp_posture.deserialize(inbuffer + offset);
00094       offset += this->grasp_posture.deserialize(inbuffer + offset);
00095       offset += this->grasp_pose.deserialize(inbuffer + offset);
00096       uint32_t * val_grasp_quality = (uint32_t*) &(this->grasp_quality);
00097       offset += 3;
00098       *val_grasp_quality = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00099       *val_grasp_quality |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00100       *val_grasp_quality |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00101       *val_grasp_quality |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00102       uint32_t exp_grasp_quality = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00103       exp_grasp_quality |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00104       if(exp_grasp_quality !=0)
00105         *val_grasp_quality |= ((exp_grasp_quality)-1023+127)<<23;
00106       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->grasp_quality = -this->grasp_quality;
00107       offset += this->approach.deserialize(inbuffer + offset);
00108       offset += this->retreat.deserialize(inbuffer + offset);
00109       union {
00110         float real;
00111         uint32_t base;
00112       } u_max_contact_force;
00113       u_max_contact_force.base = 0;
00114       u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00115       u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00116       u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00117       u_max_contact_force.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00118       this->max_contact_force = u_max_contact_force.real;
00119       offset += sizeof(this->max_contact_force);
00120       uint8_t allowed_touch_objects_lengthT = *(inbuffer + offset++);
00121       if(allowed_touch_objects_lengthT > allowed_touch_objects_length)
00122         this->allowed_touch_objects = (char**)realloc(this->allowed_touch_objects, allowed_touch_objects_lengthT * sizeof(char*));
00123       offset += 3;
00124       allowed_touch_objects_length = allowed_touch_objects_lengthT;
00125       for( uint8_t i = 0; i < allowed_touch_objects_length; i++){
00126       uint32_t length_st_allowed_touch_objects;
00127       memcpy(&length_st_allowed_touch_objects, (inbuffer + offset), sizeof(uint32_t));
00128       offset += 4;
00129       for(unsigned int k= offset; k< offset+length_st_allowed_touch_objects; ++k){
00130           inbuffer[k-1]=inbuffer[k];
00131       }
00132       inbuffer[offset+length_st_allowed_touch_objects-1]=0;
00133       this->st_allowed_touch_objects = (char *)(inbuffer + offset-1);
00134       offset += length_st_allowed_touch_objects;
00135         memcpy( &(this->allowed_touch_objects[i]), &(this->st_allowed_touch_objects), sizeof(char*));
00136       }
00137      return offset;
00138     }
00139 
00140     const char * getType(){ return "manipulation_msgs/Grasp"; };
00141     const char * getMD5(){ return "83bd11da422ea1917259ee456c0e315a"; };
00142 
00143   };
00144 
00145 }
00146 #endif


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