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