jjzak / rosserial_mbed_lib

modify for Hydro version

Dependencies:   MODSERIAL

Fork of rosserial_mbed_lib by nucho

sensor_msgs/MultiDOFJointState.h@5:8cd48977ec68, 2013-10-26 (annotated)

Committer:
jjzak
Date:
Sat Oct 26 15:38:35 2013 +0000
Revision:
5:8cd48977ec68
modify for Hydro version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjzak 5:8cd48977ec68 1 #ifndef _ROS_sensor_msgs_MultiDOFJointState_h
jjzak 5:8cd48977ec68 2 #define _ROS_sensor_msgs_MultiDOFJointState_h
jjzak 5:8cd48977ec68 3
jjzak 5:8cd48977ec68 4 #include <stdint.h>
jjzak 5:8cd48977ec68 5 #include <string.h>
jjzak 5:8cd48977ec68 6 #include <stdlib.h>
jjzak 5:8cd48977ec68 7 #include "ros/msg.h"
jjzak 5:8cd48977ec68 8 #include "std_msgs/Header.h"
jjzak 5:8cd48977ec68 9 #include "geometry_msgs/Transform.h"
jjzak 5:8cd48977ec68 10 #include "geometry_msgs/Twist.h"
jjzak 5:8cd48977ec68 11 #include "geometry_msgs/Wrench.h"
jjzak 5:8cd48977ec68 12
jjzak 5:8cd48977ec68 13 namespace sensor_msgs
jjzak 5:8cd48977ec68 14 {
jjzak 5:8cd48977ec68 15
jjzak 5:8cd48977ec68 16 class MultiDOFJointState : public ros::Msg
jjzak 5:8cd48977ec68 17 {
jjzak 5:8cd48977ec68 18 public:
jjzak 5:8cd48977ec68 19 std_msgs::Header header;
jjzak 5:8cd48977ec68 20 uint8_t joint_names_length;
jjzak 5:8cd48977ec68 21 char* st_joint_names;
jjzak 5:8cd48977ec68 22 char* * joint_names;
jjzak 5:8cd48977ec68 23 uint8_t transforms_length;
jjzak 5:8cd48977ec68 24 geometry_msgs::Transform st_transforms;
jjzak 5:8cd48977ec68 25 geometry_msgs::Transform * transforms;
jjzak 5:8cd48977ec68 26 uint8_t twist_length;
jjzak 5:8cd48977ec68 27 geometry_msgs::Twist st_twist;
jjzak 5:8cd48977ec68 28 geometry_msgs::Twist * twist;
jjzak 5:8cd48977ec68 29 uint8_t wrench_length;
jjzak 5:8cd48977ec68 30 geometry_msgs::Wrench st_wrench;
jjzak 5:8cd48977ec68 31 geometry_msgs::Wrench * wrench;
jjzak 5:8cd48977ec68 32
jjzak 5:8cd48977ec68 33 virtual int serialize(unsigned char *outbuffer) const
jjzak 5:8cd48977ec68 34 {
jjzak 5:8cd48977ec68 35 int offset = 0;
jjzak 5:8cd48977ec68 36 offset += this->header.serialize(outbuffer + offset);
jjzak 5:8cd48977ec68 37 *(outbuffer + offset++) = joint_names_length;
jjzak 5:8cd48977ec68 38 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 39 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 40 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 41 for( uint8_t i = 0; i < joint_names_length; i++){
jjzak 5:8cd48977ec68 42 uint32_t length_joint_namesi = strlen( (const char*) this->joint_names[i]);
jjzak 5:8cd48977ec68 43 memcpy(outbuffer + offset, &length_joint_namesi, sizeof(uint32_t));
jjzak 5:8cd48977ec68 44 offset += 4;
jjzak 5:8cd48977ec68 45 memcpy(outbuffer + offset, this->joint_names[i], length_joint_namesi);
jjzak 5:8cd48977ec68 46 offset += length_joint_namesi;
jjzak 5:8cd48977ec68 47 }
jjzak 5:8cd48977ec68 48 *(outbuffer + offset++) = transforms_length;
jjzak 5:8cd48977ec68 49 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 50 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 51 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 52 for( uint8_t i = 0; i < transforms_length; i++){
jjzak 5:8cd48977ec68 53 offset += this->transforms[i].serialize(outbuffer + offset);
jjzak 5:8cd48977ec68 54 }
jjzak 5:8cd48977ec68 55 *(outbuffer + offset++) = twist_length;
jjzak 5:8cd48977ec68 56 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 57 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 58 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 59 for( uint8_t i = 0; i < twist_length; i++){
jjzak 5:8cd48977ec68 60 offset += this->twist[i].serialize(outbuffer + offset);
jjzak 5:8cd48977ec68 61 }
jjzak 5:8cd48977ec68 62 *(outbuffer + offset++) = wrench_length;
jjzak 5:8cd48977ec68 63 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 64 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 65 *(outbuffer + offset++) = 0;
jjzak 5:8cd48977ec68 66 for( uint8_t i = 0; i < wrench_length; i++){
jjzak 5:8cd48977ec68 67 offset += this->wrench[i].serialize(outbuffer + offset);
jjzak 5:8cd48977ec68 68 }
jjzak 5:8cd48977ec68 69 return offset;
jjzak 5:8cd48977ec68 70 }
jjzak 5:8cd48977ec68 71
jjzak 5:8cd48977ec68 72 virtual int deserialize(unsigned char *inbuffer)
jjzak 5:8cd48977ec68 73 {
jjzak 5:8cd48977ec68 74 int offset = 0;
jjzak 5:8cd48977ec68 75 offset += this->header.deserialize(inbuffer + offset);
jjzak 5:8cd48977ec68 76 uint8_t joint_names_lengthT = *(inbuffer + offset++);
jjzak 5:8cd48977ec68 77 if(joint_names_lengthT > joint_names_length)
jjzak 5:8cd48977ec68 78 this->joint_names = (char**)realloc(this->joint_names, joint_names_lengthT * sizeof(char*));
jjzak 5:8cd48977ec68 79 offset += 3;
jjzak 5:8cd48977ec68 80 joint_names_length = joint_names_lengthT;
jjzak 5:8cd48977ec68 81 for( uint8_t i = 0; i < joint_names_length; i++){
jjzak 5:8cd48977ec68 82 uint32_t length_st_joint_names;
jjzak 5:8cd48977ec68 83 memcpy(&length_st_joint_names, (inbuffer + offset), sizeof(uint32_t));
jjzak 5:8cd48977ec68 84 offset += 4;
jjzak 5:8cd48977ec68 85 for(unsigned int k= offset; k< offset+length_st_joint_names; ++k){
jjzak 5:8cd48977ec68 86 inbuffer[k-1]=inbuffer[k];
jjzak 5:8cd48977ec68 87 }
jjzak 5:8cd48977ec68 88 inbuffer[offset+length_st_joint_names-1]=0;
jjzak 5:8cd48977ec68 89 this->st_joint_names = (char *)(inbuffer + offset-1);
jjzak 5:8cd48977ec68 90 offset += length_st_joint_names;
jjzak 5:8cd48977ec68 91 memcpy( &(this->joint_names[i]), &(this->st_joint_names), sizeof(char*));
jjzak 5:8cd48977ec68 92 }
jjzak 5:8cd48977ec68 93 uint8_t transforms_lengthT = *(inbuffer + offset++);
jjzak 5:8cd48977ec68 94 if(transforms_lengthT > transforms_length)
jjzak 5:8cd48977ec68 95 this->transforms = (geometry_msgs::Transform*)realloc(this->transforms, transforms_lengthT * sizeof(geometry_msgs::Transform));
jjzak 5:8cd48977ec68 96 offset += 3;
jjzak 5:8cd48977ec68 97 transforms_length = transforms_lengthT;
jjzak 5:8cd48977ec68 98 for( uint8_t i = 0; i < transforms_length; i++){
jjzak 5:8cd48977ec68 99 offset += this->st_transforms.deserialize(inbuffer + offset);
jjzak 5:8cd48977ec68 100 memcpy( &(this->transforms[i]), &(this->st_transforms), sizeof(geometry_msgs::Transform));
jjzak 5:8cd48977ec68 101 }
jjzak 5:8cd48977ec68 102 uint8_t twist_lengthT = *(inbuffer + offset++);
jjzak 5:8cd48977ec68 103 if(twist_lengthT > twist_length)
jjzak 5:8cd48977ec68 104 this->twist = (geometry_msgs::Twist*)realloc(this->twist, twist_lengthT * sizeof(geometry_msgs::Twist));
jjzak 5:8cd48977ec68 105 offset += 3;
jjzak 5:8cd48977ec68 106 twist_length = twist_lengthT;
jjzak 5:8cd48977ec68 107 for( uint8_t i = 0; i < twist_length; i++){
jjzak 5:8cd48977ec68 108 offset += this->st_twist.deserialize(inbuffer + offset);
jjzak 5:8cd48977ec68 109 memcpy( &(this->twist[i]), &(this->st_twist), sizeof(geometry_msgs::Twist));
jjzak 5:8cd48977ec68 110 }
jjzak 5:8cd48977ec68 111 uint8_t wrench_lengthT = *(inbuffer + offset++);
jjzak 5:8cd48977ec68 112 if(wrench_lengthT > wrench_length)
jjzak 5:8cd48977ec68 113 this->wrench = (geometry_msgs::Wrench*)realloc(this->wrench, wrench_lengthT * sizeof(geometry_msgs::Wrench));
jjzak 5:8cd48977ec68 114 offset += 3;
jjzak 5:8cd48977ec68 115 wrench_length = wrench_lengthT;
jjzak 5:8cd48977ec68 116 for( uint8_t i = 0; i < wrench_length; i++){
jjzak 5:8cd48977ec68 117 offset += this->st_wrench.deserialize(inbuffer + offset);
jjzak 5:8cd48977ec68 118 memcpy( &(this->wrench[i]), &(this->st_wrench), sizeof(geometry_msgs::Wrench));
jjzak 5:8cd48977ec68 119 }
jjzak 5:8cd48977ec68 120 return offset;
jjzak 5:8cd48977ec68 121 }
jjzak 5:8cd48977ec68 122
jjzak 5:8cd48977ec68 123 const char * getType(){ return "sensor_msgs/MultiDOFJointState"; };
jjzak 5:8cd48977ec68 124 const char * getMD5(){ return "690f272f0640d2631c305eeb8301e59d"; };
jjzak 5:8cd48977ec68 125
jjzak 5:8cd48977ec68 126 };
jjzak 5:8cd48977ec68 127
jjzak 5:8cd48977ec68 128 }
jjzak 5:8cd48977ec68 129 #endif