catchrobo2022
/
catchrobo2022_mbed
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
GetPositionFK.h
00001 #ifndef _ROS_SERVICE_GetPositionFK_h 00002 #define _ROS_SERVICE_GetPositionFK_h 00003 #include <stdint.h> 00004 #include <string.h> 00005 #include <stdlib.h> 00006 #include "ros/msg.h" 00007 #include "moveit_msgs/MoveItErrorCodes.h" 00008 #include "geometry_msgs/PoseStamped.h" 00009 #include "std_msgs/Header.h" 00010 #include "moveit_msgs/RobotState.h" 00011 00012 namespace moveit_msgs 00013 { 00014 00015 static const char GETPOSITIONFK[] = "moveit_msgs/GetPositionFK"; 00016 00017 class GetPositionFKRequest : public ros::Msg 00018 { 00019 public: 00020 typedef std_msgs::Header _header_type; 00021 _header_type header; 00022 uint32_t fk_link_names_length; 00023 typedef char* _fk_link_names_type; 00024 _fk_link_names_type st_fk_link_names; 00025 _fk_link_names_type * fk_link_names; 00026 typedef moveit_msgs::RobotState _robot_state_type; 00027 _robot_state_type robot_state; 00028 00029 GetPositionFKRequest(): 00030 header(), 00031 fk_link_names_length(0), fk_link_names(NULL), 00032 robot_state() 00033 { 00034 } 00035 00036 virtual int serialize(unsigned char *outbuffer) const 00037 { 00038 int offset = 0; 00039 offset += this->header.serialize(outbuffer + offset); 00040 *(outbuffer + offset + 0) = (this->fk_link_names_length >> (8 * 0)) & 0xFF; 00041 *(outbuffer + offset + 1) = (this->fk_link_names_length >> (8 * 1)) & 0xFF; 00042 *(outbuffer + offset + 2) = (this->fk_link_names_length >> (8 * 2)) & 0xFF; 00043 *(outbuffer + offset + 3) = (this->fk_link_names_length >> (8 * 3)) & 0xFF; 00044 offset += sizeof(this->fk_link_names_length); 00045 for( uint32_t i = 0; i < fk_link_names_length; i++){ 00046 uint32_t length_fk_link_namesi = strlen(this->fk_link_names[i]); 00047 varToArr(outbuffer + offset, length_fk_link_namesi); 00048 offset += 4; 00049 memcpy(outbuffer + offset, this->fk_link_names[i], length_fk_link_namesi); 00050 offset += length_fk_link_namesi; 00051 } 00052 offset += this->robot_state.serialize(outbuffer + offset); 00053 return offset; 00054 } 00055 00056 virtual int deserialize(unsigned char *inbuffer) 00057 { 00058 int offset = 0; 00059 offset += this->header.deserialize(inbuffer + offset); 00060 uint32_t fk_link_names_lengthT = ((uint32_t) (*(inbuffer + offset))); 00061 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00062 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00063 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00064 offset += sizeof(this->fk_link_names_length); 00065 if(fk_link_names_lengthT > fk_link_names_length) 00066 this->fk_link_names = (char**)realloc(this->fk_link_names, fk_link_names_lengthT * sizeof(char*)); 00067 fk_link_names_length = fk_link_names_lengthT; 00068 for( uint32_t i = 0; i < fk_link_names_length; i++){ 00069 uint32_t length_st_fk_link_names; 00070 arrToVar(length_st_fk_link_names, (inbuffer + offset)); 00071 offset += 4; 00072 for(unsigned int k= offset; k< offset+length_st_fk_link_names; ++k){ 00073 inbuffer[k-1]=inbuffer[k]; 00074 } 00075 inbuffer[offset+length_st_fk_link_names-1]=0; 00076 this->st_fk_link_names = (char *)(inbuffer + offset-1); 00077 offset += length_st_fk_link_names; 00078 memcpy( &(this->fk_link_names[i]), &(this->st_fk_link_names), sizeof(char*)); 00079 } 00080 offset += this->robot_state.deserialize(inbuffer + offset); 00081 return offset; 00082 } 00083 00084 virtual const char * getType(){ return GETPOSITIONFK; }; 00085 virtual const char * getMD5(){ return "1d1ed72044ed56f6246c31b522781797"; }; 00086 00087 }; 00088 00089 class GetPositionFKResponse : public ros::Msg 00090 { 00091 public: 00092 uint32_t pose_stamped_length; 00093 typedef geometry_msgs::PoseStamped _pose_stamped_type; 00094 _pose_stamped_type st_pose_stamped; 00095 _pose_stamped_type * pose_stamped; 00096 uint32_t fk_link_names_length; 00097 typedef char* _fk_link_names_type; 00098 _fk_link_names_type st_fk_link_names; 00099 _fk_link_names_type * fk_link_names; 00100 typedef moveit_msgs::MoveItErrorCodes _error_code_type; 00101 _error_code_type error_code; 00102 00103 GetPositionFKResponse(): 00104 pose_stamped_length(0), pose_stamped(NULL), 00105 fk_link_names_length(0), fk_link_names(NULL), 00106 error_code() 00107 { 00108 } 00109 00110 virtual int serialize(unsigned char *outbuffer) const 00111 { 00112 int offset = 0; 00113 *(outbuffer + offset + 0) = (this->pose_stamped_length >> (8 * 0)) & 0xFF; 00114 *(outbuffer + offset + 1) = (this->pose_stamped_length >> (8 * 1)) & 0xFF; 00115 *(outbuffer + offset + 2) = (this->pose_stamped_length >> (8 * 2)) & 0xFF; 00116 *(outbuffer + offset + 3) = (this->pose_stamped_length >> (8 * 3)) & 0xFF; 00117 offset += sizeof(this->pose_stamped_length); 00118 for( uint32_t i = 0; i < pose_stamped_length; i++){ 00119 offset += this->pose_stamped[i].serialize(outbuffer + offset); 00120 } 00121 *(outbuffer + offset + 0) = (this->fk_link_names_length >> (8 * 0)) & 0xFF; 00122 *(outbuffer + offset + 1) = (this->fk_link_names_length >> (8 * 1)) & 0xFF; 00123 *(outbuffer + offset + 2) = (this->fk_link_names_length >> (8 * 2)) & 0xFF; 00124 *(outbuffer + offset + 3) = (this->fk_link_names_length >> (8 * 3)) & 0xFF; 00125 offset += sizeof(this->fk_link_names_length); 00126 for( uint32_t i = 0; i < fk_link_names_length; i++){ 00127 uint32_t length_fk_link_namesi = strlen(this->fk_link_names[i]); 00128 varToArr(outbuffer + offset, length_fk_link_namesi); 00129 offset += 4; 00130 memcpy(outbuffer + offset, this->fk_link_names[i], length_fk_link_namesi); 00131 offset += length_fk_link_namesi; 00132 } 00133 offset += this->error_code.serialize(outbuffer + offset); 00134 return offset; 00135 } 00136 00137 virtual int deserialize(unsigned char *inbuffer) 00138 { 00139 int offset = 0; 00140 uint32_t pose_stamped_lengthT = ((uint32_t) (*(inbuffer + offset))); 00141 pose_stamped_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00142 pose_stamped_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00143 pose_stamped_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00144 offset += sizeof(this->pose_stamped_length); 00145 if(pose_stamped_lengthT > pose_stamped_length) 00146 this->pose_stamped = (geometry_msgs::PoseStamped*)realloc(this->pose_stamped, pose_stamped_lengthT * sizeof(geometry_msgs::PoseStamped)); 00147 pose_stamped_length = pose_stamped_lengthT; 00148 for( uint32_t i = 0; i < pose_stamped_length; i++){ 00149 offset += this->st_pose_stamped.deserialize(inbuffer + offset); 00150 memcpy( &(this->pose_stamped[i]), &(this->st_pose_stamped), sizeof(geometry_msgs::PoseStamped)); 00151 } 00152 uint32_t fk_link_names_lengthT = ((uint32_t) (*(inbuffer + offset))); 00153 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00154 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00155 fk_link_names_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00156 offset += sizeof(this->fk_link_names_length); 00157 if(fk_link_names_lengthT > fk_link_names_length) 00158 this->fk_link_names = (char**)realloc(this->fk_link_names, fk_link_names_lengthT * sizeof(char*)); 00159 fk_link_names_length = fk_link_names_lengthT; 00160 for( uint32_t i = 0; i < fk_link_names_length; i++){ 00161 uint32_t length_st_fk_link_names; 00162 arrToVar(length_st_fk_link_names, (inbuffer + offset)); 00163 offset += 4; 00164 for(unsigned int k= offset; k< offset+length_st_fk_link_names; ++k){ 00165 inbuffer[k-1]=inbuffer[k]; 00166 } 00167 inbuffer[offset+length_st_fk_link_names-1]=0; 00168 this->st_fk_link_names = (char *)(inbuffer + offset-1); 00169 offset += length_st_fk_link_names; 00170 memcpy( &(this->fk_link_names[i]), &(this->st_fk_link_names), sizeof(char*)); 00171 } 00172 offset += this->error_code.deserialize(inbuffer + offset); 00173 return offset; 00174 } 00175 00176 virtual const char * getType(){ return GETPOSITIONFK; }; 00177 virtual const char * getMD5(){ return "297215cf4fdfe0008356995ae621dae6"; }; 00178 00179 }; 00180 00181 class GetPositionFK { 00182 public: 00183 typedef GetPositionFKRequest Request; 00184 typedef GetPositionFKResponse Response; 00185 }; 00186 00187 } 00188 #endif
Generated on Mon Sep 26 2022 13:47:00 by
1.7.2