Go to the documentation of this file.00001 #ifndef _ROS_SERVICE_GetCartesianPath_h
00002 #define _ROS_SERVICE_GetCartesianPath_h
00003 #include <stdint.h>
00004 #include <string.h>
00005 #include <stdlib.h>
00006 #include "ros/msg.h"
00007 #include "std_msgs/Header.h"
00008 #include "moveit_msgs/RobotTrajectory.h"
00009 #include "moveit_msgs/RobotState.h"
00010 #include "moveit_msgs/MoveItErrorCodes.h"
00011 #include "geometry_msgs/Pose.h"
00012 #include "moveit_msgs/Constraints.h"
00013
00014 namespace moveit_msgs
00015 {
00016
00017 static const char GETCARTESIANPATH[] = "moveit_msgs/GetCartesianPath";
00018
00019 class GetCartesianPathRequest : public ros::Msg
00020 {
00021 public:
00022 std_msgs::Header header;
00023 moveit_msgs::RobotState start_state;
00024 const char* group_name;
00025 const char* link_name;
00026 uint8_t waypoints_length;
00027 geometry_msgs::Pose st_waypoints;
00028 geometry_msgs::Pose * waypoints;
00029 float max_step;
00030 float jump_threshold;
00031 bool avoid_collisions;
00032 moveit_msgs::Constraints path_constraints;
00033
00034 virtual int serialize(unsigned char *outbuffer) const
00035 {
00036 int offset = 0;
00037 offset += this->header.serialize(outbuffer + offset);
00038 offset += this->start_state.serialize(outbuffer + offset);
00039 uint32_t length_group_name = strlen(this->group_name);
00040 memcpy(outbuffer + offset, &length_group_name, sizeof(uint32_t));
00041 offset += 4;
00042 memcpy(outbuffer + offset, this->group_name, length_group_name);
00043 offset += length_group_name;
00044 uint32_t length_link_name = strlen(this->link_name);
00045 memcpy(outbuffer + offset, &length_link_name, sizeof(uint32_t));
00046 offset += 4;
00047 memcpy(outbuffer + offset, this->link_name, length_link_name);
00048 offset += length_link_name;
00049 *(outbuffer + offset++) = waypoints_length;
00050 *(outbuffer + offset++) = 0;
00051 *(outbuffer + offset++) = 0;
00052 *(outbuffer + offset++) = 0;
00053 for( uint8_t i = 0; i < waypoints_length; i++){
00054 offset += this->waypoints[i].serialize(outbuffer + offset);
00055 }
00056 int32_t * val_max_step = (int32_t *) &(this->max_step);
00057 int32_t exp_max_step = (((*val_max_step)>>23)&255);
00058 if(exp_max_step != 0)
00059 exp_max_step += 1023-127;
00060 int32_t sig_max_step = *val_max_step;
00061 *(outbuffer + offset++) = 0;
00062 *(outbuffer + offset++) = 0;
00063 *(outbuffer + offset++) = 0;
00064 *(outbuffer + offset++) = (sig_max_step<<5) & 0xff;
00065 *(outbuffer + offset++) = (sig_max_step>>3) & 0xff;
00066 *(outbuffer + offset++) = (sig_max_step>>11) & 0xff;
00067 *(outbuffer + offset++) = ((exp_max_step<<4) & 0xF0) | ((sig_max_step>>19)&0x0F);
00068 *(outbuffer + offset++) = (exp_max_step>>4) & 0x7F;
00069 if(this->max_step < 0) *(outbuffer + offset -1) |= 0x80;
00070 int32_t * val_jump_threshold = (int32_t *) &(this->jump_threshold);
00071 int32_t exp_jump_threshold = (((*val_jump_threshold)>>23)&255);
00072 if(exp_jump_threshold != 0)
00073 exp_jump_threshold += 1023-127;
00074 int32_t sig_jump_threshold = *val_jump_threshold;
00075 *(outbuffer + offset++) = 0;
00076 *(outbuffer + offset++) = 0;
00077 *(outbuffer + offset++) = 0;
00078 *(outbuffer + offset++) = (sig_jump_threshold<<5) & 0xff;
00079 *(outbuffer + offset++) = (sig_jump_threshold>>3) & 0xff;
00080 *(outbuffer + offset++) = (sig_jump_threshold>>11) & 0xff;
00081 *(outbuffer + offset++) = ((exp_jump_threshold<<4) & 0xF0) | ((sig_jump_threshold>>19)&0x0F);
00082 *(outbuffer + offset++) = (exp_jump_threshold>>4) & 0x7F;
00083 if(this->jump_threshold < 0) *(outbuffer + offset -1) |= 0x80;
00084 union {
00085 bool real;
00086 uint8_t base;
00087 } u_avoid_collisions;
00088 u_avoid_collisions.real = this->avoid_collisions;
00089 *(outbuffer + offset + 0) = (u_avoid_collisions.base >> (8 * 0)) & 0xFF;
00090 offset += sizeof(this->avoid_collisions);
00091 offset += this->path_constraints.serialize(outbuffer + offset);
00092 return offset;
00093 }
00094
00095 virtual int deserialize(unsigned char *inbuffer)
00096 {
00097 int offset = 0;
00098 offset += this->header.deserialize(inbuffer + offset);
00099 offset += this->start_state.deserialize(inbuffer + offset);
00100 uint32_t length_group_name;
00101 memcpy(&length_group_name, (inbuffer + offset), sizeof(uint32_t));
00102 offset += 4;
00103 for(unsigned int k= offset; k< offset+length_group_name; ++k){
00104 inbuffer[k-1]=inbuffer[k];
00105 }
00106 inbuffer[offset+length_group_name-1]=0;
00107 this->group_name = (char *)(inbuffer + offset-1);
00108 offset += length_group_name;
00109 uint32_t length_link_name;
00110 memcpy(&length_link_name, (inbuffer + offset), sizeof(uint32_t));
00111 offset += 4;
00112 for(unsigned int k= offset; k< offset+length_link_name; ++k){
00113 inbuffer[k-1]=inbuffer[k];
00114 }
00115 inbuffer[offset+length_link_name-1]=0;
00116 this->link_name = (char *)(inbuffer + offset-1);
00117 offset += length_link_name;
00118 uint8_t waypoints_lengthT = *(inbuffer + offset++);
00119 if(waypoints_lengthT > waypoints_length)
00120 this->waypoints = (geometry_msgs::Pose*)realloc(this->waypoints, waypoints_lengthT * sizeof(geometry_msgs::Pose));
00121 offset += 3;
00122 waypoints_length = waypoints_lengthT;
00123 for( uint8_t i = 0; i < waypoints_length; i++){
00124 offset += this->st_waypoints.deserialize(inbuffer + offset);
00125 memcpy( &(this->waypoints[i]), &(this->st_waypoints), sizeof(geometry_msgs::Pose));
00126 }
00127 uint32_t * val_max_step = (uint32_t*) &(this->max_step);
00128 offset += 3;
00129 *val_max_step = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00130 *val_max_step |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00131 *val_max_step |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00132 *val_max_step |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00133 uint32_t exp_max_step = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00134 exp_max_step |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00135 if(exp_max_step !=0)
00136 *val_max_step |= ((exp_max_step)-1023+127)<<23;
00137 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->max_step = -this->max_step;
00138 uint32_t * val_jump_threshold = (uint32_t*) &(this->jump_threshold);
00139 offset += 3;
00140 *val_jump_threshold = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00141 *val_jump_threshold |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00142 *val_jump_threshold |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00143 *val_jump_threshold |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00144 uint32_t exp_jump_threshold = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00145 exp_jump_threshold |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00146 if(exp_jump_threshold !=0)
00147 *val_jump_threshold |= ((exp_jump_threshold)-1023+127)<<23;
00148 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->jump_threshold = -this->jump_threshold;
00149 union {
00150 bool real;
00151 uint8_t base;
00152 } u_avoid_collisions;
00153 u_avoid_collisions.base = 0;
00154 u_avoid_collisions.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0);
00155 this->avoid_collisions = u_avoid_collisions.real;
00156 offset += sizeof(this->avoid_collisions);
00157 offset += this->path_constraints.deserialize(inbuffer + offset);
00158 return offset;
00159 }
00160
00161 const char * getType(){ return GETCARTESIANPATH; };
00162 const char * getMD5(){ return "b37c16ad7ed838d811a270a8054276b6"; };
00163
00164 };
00165
00166 class GetCartesianPathResponse : public ros::Msg
00167 {
00168 public:
00169 moveit_msgs::RobotState start_state;
00170 moveit_msgs::RobotTrajectory solution;
00171 float fraction;
00172 moveit_msgs::MoveItErrorCodes error_code;
00173
00174 virtual int serialize(unsigned char *outbuffer) const
00175 {
00176 int offset = 0;
00177 offset += this->start_state.serialize(outbuffer + offset);
00178 offset += this->solution.serialize(outbuffer + offset);
00179 int32_t * val_fraction = (int32_t *) &(this->fraction);
00180 int32_t exp_fraction = (((*val_fraction)>>23)&255);
00181 if(exp_fraction != 0)
00182 exp_fraction += 1023-127;
00183 int32_t sig_fraction = *val_fraction;
00184 *(outbuffer + offset++) = 0;
00185 *(outbuffer + offset++) = 0;
00186 *(outbuffer + offset++) = 0;
00187 *(outbuffer + offset++) = (sig_fraction<<5) & 0xff;
00188 *(outbuffer + offset++) = (sig_fraction>>3) & 0xff;
00189 *(outbuffer + offset++) = (sig_fraction>>11) & 0xff;
00190 *(outbuffer + offset++) = ((exp_fraction<<4) & 0xF0) | ((sig_fraction>>19)&0x0F);
00191 *(outbuffer + offset++) = (exp_fraction>>4) & 0x7F;
00192 if(this->fraction < 0) *(outbuffer + offset -1) |= 0x80;
00193 offset += this->error_code.serialize(outbuffer + offset);
00194 return offset;
00195 }
00196
00197 virtual int deserialize(unsigned char *inbuffer)
00198 {
00199 int offset = 0;
00200 offset += this->start_state.deserialize(inbuffer + offset);
00201 offset += this->solution.deserialize(inbuffer + offset);
00202 uint32_t * val_fraction = (uint32_t*) &(this->fraction);
00203 offset += 3;
00204 *val_fraction = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00205 *val_fraction |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00206 *val_fraction |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00207 *val_fraction |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00208 uint32_t exp_fraction = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00209 exp_fraction |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00210 if(exp_fraction !=0)
00211 *val_fraction |= ((exp_fraction)-1023+127)<<23;
00212 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->fraction = -this->fraction;
00213 offset += this->error_code.deserialize(inbuffer + offset);
00214 return offset;
00215 }
00216
00217 const char * getType(){ return GETCARTESIANPATH; };
00218 const char * getMD5(){ return "45414110461a45eb0e273e013924ce59"; };
00219
00220 };
00221
00222 class GetCartesianPath {
00223 public:
00224 typedef GetCartesianPathRequest Request;
00225 typedef GetCartesianPathResponse Response;
00226 };
00227
00228 }
00229 #endif