ROS Serial library for Mbed platforms for ROS Jade Turtle. Check http://wiki.ros.org/rosserial_mbed/ for more information.
Dependents: rosserial_mbed_hello_world_publisher_jade
ContactState.h
00001 #ifndef _ROS_gazebo_msgs_ContactState_h 00002 #define _ROS_gazebo_msgs_ContactState_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "geometry_msgs/Wrench.h" 00009 #include "geometry_msgs/Vector3.h" 00010 00011 namespace gazebo_msgs 00012 { 00013 00014 class ContactState : public ros::Msg 00015 { 00016 public: 00017 const char* info; 00018 const char* collision1_name; 00019 const char* collision2_name; 00020 uint8_t wrenches_length; 00021 geometry_msgs::Wrench st_wrenches; 00022 geometry_msgs::Wrench * wrenches; 00023 geometry_msgs::Wrench total_wrench; 00024 uint8_t contact_positions_length; 00025 geometry_msgs::Vector3 st_contact_positions; 00026 geometry_msgs::Vector3 * contact_positions; 00027 uint8_t contact_normals_length; 00028 geometry_msgs::Vector3 st_contact_normals; 00029 geometry_msgs::Vector3 * contact_normals; 00030 uint8_t depths_length; 00031 double st_depths; 00032 double * depths; 00033 00034 ContactState(): 00035 info(""), 00036 collision1_name(""), 00037 collision2_name(""), 00038 wrenches_length(0), wrenches(NULL), 00039 total_wrench(), 00040 contact_positions_length(0), contact_positions(NULL), 00041 contact_normals_length(0), contact_normals(NULL), 00042 depths_length(0), depths(NULL) 00043 { 00044 } 00045 00046 virtual int serialize(unsigned char *outbuffer) const 00047 { 00048 int offset = 0; 00049 uint32_t length_info = strlen(this->info); 00050 memcpy(outbuffer + offset, &length_info, sizeof(uint32_t)); 00051 offset += 4; 00052 memcpy(outbuffer + offset, this->info, length_info); 00053 offset += length_info; 00054 uint32_t length_collision1_name = strlen(this->collision1_name); 00055 memcpy(outbuffer + offset, &length_collision1_name, sizeof(uint32_t)); 00056 offset += 4; 00057 memcpy(outbuffer + offset, this->collision1_name, length_collision1_name); 00058 offset += length_collision1_name; 00059 uint32_t length_collision2_name = strlen(this->collision2_name); 00060 memcpy(outbuffer + offset, &length_collision2_name, sizeof(uint32_t)); 00061 offset += 4; 00062 memcpy(outbuffer + offset, this->collision2_name, length_collision2_name); 00063 offset += length_collision2_name; 00064 *(outbuffer + offset++) = wrenches_length; 00065 *(outbuffer + offset++) = 0; 00066 *(outbuffer + offset++) = 0; 00067 *(outbuffer + offset++) = 0; 00068 for( uint8_t i = 0; i < wrenches_length; i++){ 00069 offset += this->wrenches[i].serialize(outbuffer + offset); 00070 } 00071 offset += this->total_wrench.serialize(outbuffer + offset); 00072 *(outbuffer + offset++) = contact_positions_length; 00073 *(outbuffer + offset++) = 0; 00074 *(outbuffer + offset++) = 0; 00075 *(outbuffer + offset++) = 0; 00076 for( uint8_t i = 0; i < contact_positions_length; i++){ 00077 offset += this->contact_positions[i].serialize(outbuffer + offset); 00078 } 00079 *(outbuffer + offset++) = contact_normals_length; 00080 *(outbuffer + offset++) = 0; 00081 *(outbuffer + offset++) = 0; 00082 *(outbuffer + offset++) = 0; 00083 for( uint8_t i = 0; i < contact_normals_length; i++){ 00084 offset += this->contact_normals[i].serialize(outbuffer + offset); 00085 } 00086 *(outbuffer + offset++) = depths_length; 00087 *(outbuffer + offset++) = 0; 00088 *(outbuffer + offset++) = 0; 00089 *(outbuffer + offset++) = 0; 00090 for( uint8_t i = 0; i < depths_length; i++){ 00091 union { 00092 double real; 00093 uint64_t base; 00094 } u_depthsi; 00095 u_depthsi.real = this->depths[i]; 00096 *(outbuffer + offset + 0) = (u_depthsi.base >> (8 * 0)) & 0xFF; 00097 *(outbuffer + offset + 1) = (u_depthsi.base >> (8 * 1)) & 0xFF; 00098 *(outbuffer + offset + 2) = (u_depthsi.base >> (8 * 2)) & 0xFF; 00099 *(outbuffer + offset + 3) = (u_depthsi.base >> (8 * 3)) & 0xFF; 00100 *(outbuffer + offset + 4) = (u_depthsi.base >> (8 * 4)) & 0xFF; 00101 *(outbuffer + offset + 5) = (u_depthsi.base >> (8 * 5)) & 0xFF; 00102 *(outbuffer + offset + 6) = (u_depthsi.base >> (8 * 6)) & 0xFF; 00103 *(outbuffer + offset + 7) = (u_depthsi.base >> (8 * 7)) & 0xFF; 00104 offset += sizeof(this->depths[i]); 00105 } 00106 return offset; 00107 } 00108 00109 virtual int deserialize(unsigned char *inbuffer) 00110 { 00111 int offset = 0; 00112 uint32_t length_info; 00113 memcpy(&length_info, (inbuffer + offset), sizeof(uint32_t)); 00114 offset += 4; 00115 for(unsigned int k= offset; k< offset+length_info; ++k){ 00116 inbuffer[k-1]=inbuffer[k]; 00117 } 00118 inbuffer[offset+length_info-1]=0; 00119 this->info = (char *)(inbuffer + offset-1); 00120 offset += length_info; 00121 uint32_t length_collision1_name; 00122 memcpy(&length_collision1_name, (inbuffer + offset), sizeof(uint32_t)); 00123 offset += 4; 00124 for(unsigned int k= offset; k< offset+length_collision1_name; ++k){ 00125 inbuffer[k-1]=inbuffer[k]; 00126 } 00127 inbuffer[offset+length_collision1_name-1]=0; 00128 this->collision1_name = (char *)(inbuffer + offset-1); 00129 offset += length_collision1_name; 00130 uint32_t length_collision2_name; 00131 memcpy(&length_collision2_name, (inbuffer + offset), sizeof(uint32_t)); 00132 offset += 4; 00133 for(unsigned int k= offset; k< offset+length_collision2_name; ++k){ 00134 inbuffer[k-1]=inbuffer[k]; 00135 } 00136 inbuffer[offset+length_collision2_name-1]=0; 00137 this->collision2_name = (char *)(inbuffer + offset-1); 00138 offset += length_collision2_name; 00139 uint8_t wrenches_lengthT = *(inbuffer + offset++); 00140 if(wrenches_lengthT > wrenches_length) 00141 this->wrenches = (geometry_msgs::Wrench*)realloc(this->wrenches, wrenches_lengthT * sizeof(geometry_msgs::Wrench)); 00142 offset += 3; 00143 wrenches_length = wrenches_lengthT; 00144 for( uint8_t i = 0; i < wrenches_length; i++){ 00145 offset += this->st_wrenches.deserialize(inbuffer + offset); 00146 memcpy( &(this->wrenches[i]), &(this->st_wrenches), sizeof(geometry_msgs::Wrench)); 00147 } 00148 offset += this->total_wrench.deserialize(inbuffer + offset); 00149 uint8_t contact_positions_lengthT = *(inbuffer + offset++); 00150 if(contact_positions_lengthT > contact_positions_length) 00151 this->contact_positions = (geometry_msgs::Vector3*)realloc(this->contact_positions, contact_positions_lengthT * sizeof(geometry_msgs::Vector3)); 00152 offset += 3; 00153 contact_positions_length = contact_positions_lengthT; 00154 for( uint8_t i = 0; i < contact_positions_length; i++){ 00155 offset += this->st_contact_positions.deserialize(inbuffer + offset); 00156 memcpy( &(this->contact_positions[i]), &(this->st_contact_positions), sizeof(geometry_msgs::Vector3)); 00157 } 00158 uint8_t contact_normals_lengthT = *(inbuffer + offset++); 00159 if(contact_normals_lengthT > contact_normals_length) 00160 this->contact_normals = (geometry_msgs::Vector3*)realloc(this->contact_normals, contact_normals_lengthT * sizeof(geometry_msgs::Vector3)); 00161 offset += 3; 00162 contact_normals_length = contact_normals_lengthT; 00163 for( uint8_t i = 0; i < contact_normals_length; i++){ 00164 offset += this->st_contact_normals.deserialize(inbuffer + offset); 00165 memcpy( &(this->contact_normals[i]), &(this->st_contact_normals), sizeof(geometry_msgs::Vector3)); 00166 } 00167 uint8_t depths_lengthT = *(inbuffer + offset++); 00168 if(depths_lengthT > depths_length) 00169 this->depths = (double*)realloc(this->depths, depths_lengthT * sizeof(double)); 00170 offset += 3; 00171 depths_length = depths_lengthT; 00172 for( uint8_t i = 0; i < depths_length; i++){ 00173 union { 00174 double real; 00175 uint64_t base; 00176 } u_st_depths; 00177 u_st_depths.base = 0; 00178 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 0))) << (8 * 0); 00179 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 1))) << (8 * 1); 00180 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 2))) << (8 * 2); 00181 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 3))) << (8 * 3); 00182 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 4))) << (8 * 4); 00183 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 5))) << (8 * 5); 00184 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 6))) << (8 * 6); 00185 u_st_depths.base |= ((uint64_t) (*(inbuffer + offset + 7))) << (8 * 7); 00186 this->st_depths = u_st_depths.real; 00187 offset += sizeof(this->st_depths); 00188 memcpy( &(this->depths[i]), &(this->st_depths), sizeof(double)); 00189 } 00190 return offset; 00191 } 00192 00193 const char * getType(){ return "gazebo_msgs/ContactState"; }; 00194 const char * getMD5(){ return "48c0ffb054b8c444f870cecea1ee50d9"; }; 00195 00196 }; 00197 00198 } 00199 #endif
Generated on Tue Jul 12 2022 17:28:47 by 1.7.2