JointTrajectoryPoint.h
Go to the documentation of this file.
00001 #ifndef _ROS_trajectory_msgs_JointTrajectoryPoint_h
00002 #define _ROS_trajectory_msgs_JointTrajectoryPoint_h
00003 
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "ros/duration.h"
00009 
00010 namespace trajectory_msgs
00011 {
00012 
00013   class JointTrajectoryPoint : public ros::Msg
00014   {
00015     public:
00016       uint8_t positions_length;
00017       float st_positions;
00018       float * positions;
00019       uint8_t velocities_length;
00020       float st_velocities;
00021       float * velocities;
00022       uint8_t accelerations_length;
00023       float st_accelerations;
00024       float * accelerations;
00025       uint8_t effort_length;
00026       float st_effort;
00027       float * effort;
00028       ros::Duration time_from_start;
00029 
00030     virtual int serialize(unsigned char *outbuffer) const
00031     {
00032       int offset = 0;
00033       *(outbuffer + offset++) = positions_length;
00034       *(outbuffer + offset++) = 0;
00035       *(outbuffer + offset++) = 0;
00036       *(outbuffer + offset++) = 0;
00037       for( uint8_t i = 0; i < positions_length; i++){
00038       int32_t * val_positionsi = (int32_t *) &(this->positions[i]);
00039       int32_t exp_positionsi = (((*val_positionsi)>>23)&255);
00040       if(exp_positionsi != 0)
00041         exp_positionsi += 1023-127;
00042       int32_t sig_positionsi = *val_positionsi;
00043       *(outbuffer + offset++) = 0;
00044       *(outbuffer + offset++) = 0;
00045       *(outbuffer + offset++) = 0;
00046       *(outbuffer + offset++) = (sig_positionsi<<5) & 0xff;
00047       *(outbuffer + offset++) = (sig_positionsi>>3) & 0xff;
00048       *(outbuffer + offset++) = (sig_positionsi>>11) & 0xff;
00049       *(outbuffer + offset++) = ((exp_positionsi<<4) & 0xF0) | ((sig_positionsi>>19)&0x0F);
00050       *(outbuffer + offset++) = (exp_positionsi>>4) & 0x7F;
00051       if(this->positions[i] < 0) *(outbuffer + offset -1) |= 0x80;
00052       }
00053       *(outbuffer + offset++) = velocities_length;
00054       *(outbuffer + offset++) = 0;
00055       *(outbuffer + offset++) = 0;
00056       *(outbuffer + offset++) = 0;
00057       for( uint8_t i = 0; i < velocities_length; i++){
00058       int32_t * val_velocitiesi = (int32_t *) &(this->velocities[i]);
00059       int32_t exp_velocitiesi = (((*val_velocitiesi)>>23)&255);
00060       if(exp_velocitiesi != 0)
00061         exp_velocitiesi += 1023-127;
00062       int32_t sig_velocitiesi = *val_velocitiesi;
00063       *(outbuffer + offset++) = 0;
00064       *(outbuffer + offset++) = 0;
00065       *(outbuffer + offset++) = 0;
00066       *(outbuffer + offset++) = (sig_velocitiesi<<5) & 0xff;
00067       *(outbuffer + offset++) = (sig_velocitiesi>>3) & 0xff;
00068       *(outbuffer + offset++) = (sig_velocitiesi>>11) & 0xff;
00069       *(outbuffer + offset++) = ((exp_velocitiesi<<4) & 0xF0) | ((sig_velocitiesi>>19)&0x0F);
00070       *(outbuffer + offset++) = (exp_velocitiesi>>4) & 0x7F;
00071       if(this->velocities[i] < 0) *(outbuffer + offset -1) |= 0x80;
00072       }
00073       *(outbuffer + offset++) = accelerations_length;
00074       *(outbuffer + offset++) = 0;
00075       *(outbuffer + offset++) = 0;
00076       *(outbuffer + offset++) = 0;
00077       for( uint8_t i = 0; i < accelerations_length; i++){
00078       int32_t * val_accelerationsi = (int32_t *) &(this->accelerations[i]);
00079       int32_t exp_accelerationsi = (((*val_accelerationsi)>>23)&255);
00080       if(exp_accelerationsi != 0)
00081         exp_accelerationsi += 1023-127;
00082       int32_t sig_accelerationsi = *val_accelerationsi;
00083       *(outbuffer + offset++) = 0;
00084       *(outbuffer + offset++) = 0;
00085       *(outbuffer + offset++) = 0;
00086       *(outbuffer + offset++) = (sig_accelerationsi<<5) & 0xff;
00087       *(outbuffer + offset++) = (sig_accelerationsi>>3) & 0xff;
00088       *(outbuffer + offset++) = (sig_accelerationsi>>11) & 0xff;
00089       *(outbuffer + offset++) = ((exp_accelerationsi<<4) & 0xF0) | ((sig_accelerationsi>>19)&0x0F);
00090       *(outbuffer + offset++) = (exp_accelerationsi>>4) & 0x7F;
00091       if(this->accelerations[i] < 0) *(outbuffer + offset -1) |= 0x80;
00092       }
00093       *(outbuffer + offset++) = effort_length;
00094       *(outbuffer + offset++) = 0;
00095       *(outbuffer + offset++) = 0;
00096       *(outbuffer + offset++) = 0;
00097       for( uint8_t i = 0; i < effort_length; i++){
00098       int32_t * val_efforti = (int32_t *) &(this->effort[i]);
00099       int32_t exp_efforti = (((*val_efforti)>>23)&255);
00100       if(exp_efforti != 0)
00101         exp_efforti += 1023-127;
00102       int32_t sig_efforti = *val_efforti;
00103       *(outbuffer + offset++) = 0;
00104       *(outbuffer + offset++) = 0;
00105       *(outbuffer + offset++) = 0;
00106       *(outbuffer + offset++) = (sig_efforti<<5) & 0xff;
00107       *(outbuffer + offset++) = (sig_efforti>>3) & 0xff;
00108       *(outbuffer + offset++) = (sig_efforti>>11) & 0xff;
00109       *(outbuffer + offset++) = ((exp_efforti<<4) & 0xF0) | ((sig_efforti>>19)&0x0F);
00110       *(outbuffer + offset++) = (exp_efforti>>4) & 0x7F;
00111       if(this->effort[i] < 0) *(outbuffer + offset -1) |= 0x80;
00112       }
00113       *(outbuffer + offset + 0) = (this->time_from_start.sec >> (8 * 0)) & 0xFF;
00114       *(outbuffer + offset + 1) = (this->time_from_start.sec >> (8 * 1)) & 0xFF;
00115       *(outbuffer + offset + 2) = (this->time_from_start.sec >> (8 * 2)) & 0xFF;
00116       *(outbuffer + offset + 3) = (this->time_from_start.sec >> (8 * 3)) & 0xFF;
00117       offset += sizeof(this->time_from_start.sec);
00118       *(outbuffer + offset + 0) = (this->time_from_start.nsec >> (8 * 0)) & 0xFF;
00119       *(outbuffer + offset + 1) = (this->time_from_start.nsec >> (8 * 1)) & 0xFF;
00120       *(outbuffer + offset + 2) = (this->time_from_start.nsec >> (8 * 2)) & 0xFF;
00121       *(outbuffer + offset + 3) = (this->time_from_start.nsec >> (8 * 3)) & 0xFF;
00122       offset += sizeof(this->time_from_start.nsec);
00123       return offset;
00124     }
00125 
00126     virtual int deserialize(unsigned char *inbuffer)
00127     {
00128       int offset = 0;
00129       uint8_t positions_lengthT = *(inbuffer + offset++);
00130       if(positions_lengthT > positions_length)
00131         this->positions = (float*)realloc(this->positions, positions_lengthT * sizeof(float));
00132       offset += 3;
00133       positions_length = positions_lengthT;
00134       for( uint8_t i = 0; i < positions_length; i++){
00135       uint32_t * val_st_positions = (uint32_t*) &(this->st_positions);
00136       offset += 3;
00137       *val_st_positions = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00138       *val_st_positions |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00139       *val_st_positions |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00140       *val_st_positions |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00141       uint32_t exp_st_positions = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00142       exp_st_positions |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00143       if(exp_st_positions !=0)
00144         *val_st_positions |= ((exp_st_positions)-1023+127)<<23;
00145       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_positions = -this->st_positions;
00146         memcpy( &(this->positions[i]), &(this->st_positions), sizeof(float));
00147       }
00148       uint8_t velocities_lengthT = *(inbuffer + offset++);
00149       if(velocities_lengthT > velocities_length)
00150         this->velocities = (float*)realloc(this->velocities, velocities_lengthT * sizeof(float));
00151       offset += 3;
00152       velocities_length = velocities_lengthT;
00153       for( uint8_t i = 0; i < velocities_length; i++){
00154       uint32_t * val_st_velocities = (uint32_t*) &(this->st_velocities);
00155       offset += 3;
00156       *val_st_velocities = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00157       *val_st_velocities |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00158       *val_st_velocities |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00159       *val_st_velocities |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00160       uint32_t exp_st_velocities = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00161       exp_st_velocities |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00162       if(exp_st_velocities !=0)
00163         *val_st_velocities |= ((exp_st_velocities)-1023+127)<<23;
00164       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_velocities = -this->st_velocities;
00165         memcpy( &(this->velocities[i]), &(this->st_velocities), sizeof(float));
00166       }
00167       uint8_t accelerations_lengthT = *(inbuffer + offset++);
00168       if(accelerations_lengthT > accelerations_length)
00169         this->accelerations = (float*)realloc(this->accelerations, accelerations_lengthT * sizeof(float));
00170       offset += 3;
00171       accelerations_length = accelerations_lengthT;
00172       for( uint8_t i = 0; i < accelerations_length; i++){
00173       uint32_t * val_st_accelerations = (uint32_t*) &(this->st_accelerations);
00174       offset += 3;
00175       *val_st_accelerations = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00176       *val_st_accelerations |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00177       *val_st_accelerations |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00178       *val_st_accelerations |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00179       uint32_t exp_st_accelerations = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00180       exp_st_accelerations |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00181       if(exp_st_accelerations !=0)
00182         *val_st_accelerations |= ((exp_st_accelerations)-1023+127)<<23;
00183       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_accelerations = -this->st_accelerations;
00184         memcpy( &(this->accelerations[i]), &(this->st_accelerations), sizeof(float));
00185       }
00186       uint8_t effort_lengthT = *(inbuffer + offset++);
00187       if(effort_lengthT > effort_length)
00188         this->effort = (float*)realloc(this->effort, effort_lengthT * sizeof(float));
00189       offset += 3;
00190       effort_length = effort_lengthT;
00191       for( uint8_t i = 0; i < effort_length; i++){
00192       uint32_t * val_st_effort = (uint32_t*) &(this->st_effort);
00193       offset += 3;
00194       *val_st_effort = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00195       *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00196       *val_st_effort |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00197       *val_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00198       uint32_t exp_st_effort = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00199       exp_st_effort |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00200       if(exp_st_effort !=0)
00201         *val_st_effort |= ((exp_st_effort)-1023+127)<<23;
00202       if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_effort = -this->st_effort;
00203         memcpy( &(this->effort[i]), &(this->st_effort), sizeof(float));
00204       }
00205       this->time_from_start.sec =  ((uint32_t) (*(inbuffer + offset)));
00206       this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00207       this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00208       this->time_from_start.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00209       offset += sizeof(this->time_from_start.sec);
00210       this->time_from_start.nsec =  ((uint32_t) (*(inbuffer + offset)));
00211       this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00212       this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00213       this->time_from_start.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00214       offset += sizeof(this->time_from_start.nsec);
00215      return offset;
00216     }
00217 
00218     const char * getType(){ return "trajectory_msgs/JointTrajectoryPoint"; };
00219     const char * getMD5(){ return "f3cd1e1c4d320c79d6985c904ae5dcd3"; };
00220 
00221   };
00222 
00223 }
00224 #endif


lizi_arduino
Author(s): RoboTiCan
autogenerated on Wed Aug 26 2015 12:24:22