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