modify for Hydro version
Fork of rosserial_mbed_lib by
dianostic_msgs/SelfTest.h@6:3c54bc7badd4, 2013-10-26 (annotated)
- Committer:
- jjzak
- Date:
- Sat Oct 26 15:39:01 2013 +0000
- Revision:
- 6:3c54bc7badd4
modify for Hydro version;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jjzak | 6:3c54bc7badd4 | 1 | #ifndef _ROS_SERVICE_SelfTest_h |
jjzak | 6:3c54bc7badd4 | 2 | #define _ROS_SERVICE_SelfTest_h |
jjzak | 6:3c54bc7badd4 | 3 | #include <stdint.h> |
jjzak | 6:3c54bc7badd4 | 4 | #include <string.h> |
jjzak | 6:3c54bc7badd4 | 5 | #include <stdlib.h> |
jjzak | 6:3c54bc7badd4 | 6 | #include "ros/msg.h" |
jjzak | 6:3c54bc7badd4 | 7 | #include "diagnostic_msgs/DiagnosticStatus.h" |
jjzak | 6:3c54bc7badd4 | 8 | |
jjzak | 6:3c54bc7badd4 | 9 | namespace diagnostic_msgs |
jjzak | 6:3c54bc7badd4 | 10 | { |
jjzak | 6:3c54bc7badd4 | 11 | |
jjzak | 6:3c54bc7badd4 | 12 | static const char SELFTEST[] = "diagnostic_msgs/SelfTest"; |
jjzak | 6:3c54bc7badd4 | 13 | |
jjzak | 6:3c54bc7badd4 | 14 | class SelfTestRequest : public ros::Msg |
jjzak | 6:3c54bc7badd4 | 15 | { |
jjzak | 6:3c54bc7badd4 | 16 | public: |
jjzak | 6:3c54bc7badd4 | 17 | |
jjzak | 6:3c54bc7badd4 | 18 | virtual int serialize(unsigned char *outbuffer) const |
jjzak | 6:3c54bc7badd4 | 19 | { |
jjzak | 6:3c54bc7badd4 | 20 | int offset = 0; |
jjzak | 6:3c54bc7badd4 | 21 | return offset; |
jjzak | 6:3c54bc7badd4 | 22 | } |
jjzak | 6:3c54bc7badd4 | 23 | |
jjzak | 6:3c54bc7badd4 | 24 | virtual int deserialize(unsigned char *inbuffer) |
jjzak | 6:3c54bc7badd4 | 25 | { |
jjzak | 6:3c54bc7badd4 | 26 | int offset = 0; |
jjzak | 6:3c54bc7badd4 | 27 | return offset; |
jjzak | 6:3c54bc7badd4 | 28 | } |
jjzak | 6:3c54bc7badd4 | 29 | |
jjzak | 6:3c54bc7badd4 | 30 | const char * getType(){ return SELFTEST; }; |
jjzak | 6:3c54bc7badd4 | 31 | const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; |
jjzak | 6:3c54bc7badd4 | 32 | |
jjzak | 6:3c54bc7badd4 | 33 | }; |
jjzak | 6:3c54bc7badd4 | 34 | |
jjzak | 6:3c54bc7badd4 | 35 | class SelfTestResponse : public ros::Msg |
jjzak | 6:3c54bc7badd4 | 36 | { |
jjzak | 6:3c54bc7badd4 | 37 | public: |
jjzak | 6:3c54bc7badd4 | 38 | char * id; |
jjzak | 6:3c54bc7badd4 | 39 | int8_t passed; |
jjzak | 6:3c54bc7badd4 | 40 | uint8_t status_length; |
jjzak | 6:3c54bc7badd4 | 41 | diagnostic_msgs::DiagnosticStatus st_status; |
jjzak | 6:3c54bc7badd4 | 42 | diagnostic_msgs::DiagnosticStatus * status; |
jjzak | 6:3c54bc7badd4 | 43 | |
jjzak | 6:3c54bc7badd4 | 44 | virtual int serialize(unsigned char *outbuffer) const |
jjzak | 6:3c54bc7badd4 | 45 | { |
jjzak | 6:3c54bc7badd4 | 46 | int offset = 0; |
jjzak | 6:3c54bc7badd4 | 47 | uint32_t length_id = strlen( (const char*) this->id); |
jjzak | 6:3c54bc7badd4 | 48 | memcpy(outbuffer + offset, &length_id, sizeof(uint32_t)); |
jjzak | 6:3c54bc7badd4 | 49 | offset += 4; |
jjzak | 6:3c54bc7badd4 | 50 | memcpy(outbuffer + offset, this->id, length_id); |
jjzak | 6:3c54bc7badd4 | 51 | offset += length_id; |
jjzak | 6:3c54bc7badd4 | 52 | union { |
jjzak | 6:3c54bc7badd4 | 53 | int8_t real; |
jjzak | 6:3c54bc7badd4 | 54 | uint8_t base; |
jjzak | 6:3c54bc7badd4 | 55 | } u_passed; |
jjzak | 6:3c54bc7badd4 | 56 | u_passed.real = this->passed; |
jjzak | 6:3c54bc7badd4 | 57 | *(outbuffer + offset + 0) = (u_passed.base >> (8 * 0)) & 0xFF; |
jjzak | 6:3c54bc7badd4 | 58 | offset += sizeof(this->passed); |
jjzak | 6:3c54bc7badd4 | 59 | *(outbuffer + offset++) = status_length; |
jjzak | 6:3c54bc7badd4 | 60 | *(outbuffer + offset++) = 0; |
jjzak | 6:3c54bc7badd4 | 61 | *(outbuffer + offset++) = 0; |
jjzak | 6:3c54bc7badd4 | 62 | *(outbuffer + offset++) = 0; |
jjzak | 6:3c54bc7badd4 | 63 | for( uint8_t i = 0; i < status_length; i++){ |
jjzak | 6:3c54bc7badd4 | 64 | offset += this->status[i].serialize(outbuffer + offset); |
jjzak | 6:3c54bc7badd4 | 65 | } |
jjzak | 6:3c54bc7badd4 | 66 | return offset; |
jjzak | 6:3c54bc7badd4 | 67 | } |
jjzak | 6:3c54bc7badd4 | 68 | |
jjzak | 6:3c54bc7badd4 | 69 | virtual int deserialize(unsigned char *inbuffer) |
jjzak | 6:3c54bc7badd4 | 70 | { |
jjzak | 6:3c54bc7badd4 | 71 | int offset = 0; |
jjzak | 6:3c54bc7badd4 | 72 | uint32_t length_id; |
jjzak | 6:3c54bc7badd4 | 73 | memcpy(&length_id, (inbuffer + offset), sizeof(uint32_t)); |
jjzak | 6:3c54bc7badd4 | 74 | offset += 4; |
jjzak | 6:3c54bc7badd4 | 75 | for(unsigned int k= offset; k< offset+length_id; ++k){ |
jjzak | 6:3c54bc7badd4 | 76 | inbuffer[k-1]=inbuffer[k]; |
jjzak | 6:3c54bc7badd4 | 77 | } |
jjzak | 6:3c54bc7badd4 | 78 | inbuffer[offset+length_id-1]=0; |
jjzak | 6:3c54bc7badd4 | 79 | this->id = (char *)(inbuffer + offset-1); |
jjzak | 6:3c54bc7badd4 | 80 | offset += length_id; |
jjzak | 6:3c54bc7badd4 | 81 | union { |
jjzak | 6:3c54bc7badd4 | 82 | int8_t real; |
jjzak | 6:3c54bc7badd4 | 83 | uint8_t base; |
jjzak | 6:3c54bc7badd4 | 84 | } u_passed; |
jjzak | 6:3c54bc7badd4 | 85 | u_passed.base = 0; |
jjzak | 6:3c54bc7badd4 | 86 | u_passed.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); |
jjzak | 6:3c54bc7badd4 | 87 | this->passed = u_passed.real; |
jjzak | 6:3c54bc7badd4 | 88 | offset += sizeof(this->passed); |
jjzak | 6:3c54bc7badd4 | 89 | uint8_t status_lengthT = *(inbuffer + offset++); |
jjzak | 6:3c54bc7badd4 | 90 | if(status_lengthT > status_length) |
jjzak | 6:3c54bc7badd4 | 91 | this->status = (diagnostic_msgs::DiagnosticStatus*)realloc(this->status, status_lengthT * sizeof(diagnostic_msgs::DiagnosticStatus)); |
jjzak | 6:3c54bc7badd4 | 92 | offset += 3; |
jjzak | 6:3c54bc7badd4 | 93 | status_length = status_lengthT; |
jjzak | 6:3c54bc7badd4 | 94 | for( uint8_t i = 0; i < status_length; i++){ |
jjzak | 6:3c54bc7badd4 | 95 | offset += this->st_status.deserialize(inbuffer + offset); |
jjzak | 6:3c54bc7badd4 | 96 | memcpy( &(this->status[i]), &(this->st_status), sizeof(diagnostic_msgs::DiagnosticStatus)); |
jjzak | 6:3c54bc7badd4 | 97 | } |
jjzak | 6:3c54bc7badd4 | 98 | return offset; |
jjzak | 6:3c54bc7badd4 | 99 | } |
jjzak | 6:3c54bc7badd4 | 100 | |
jjzak | 6:3c54bc7badd4 | 101 | const char * getType(){ return SELFTEST; }; |
jjzak | 6:3c54bc7badd4 | 102 | const char * getMD5(){ return "74c9372c870a76da4fc2b3973978b898"; }; |
jjzak | 6:3c54bc7badd4 | 103 | |
jjzak | 6:3c54bc7badd4 | 104 | }; |
jjzak | 6:3c54bc7badd4 | 105 | |
jjzak | 6:3c54bc7badd4 | 106 | class SelfTest { |
jjzak | 6:3c54bc7badd4 | 107 | public: |
jjzak | 6:3c54bc7badd4 | 108 | typedef SelfTestRequest Request; |
jjzak | 6:3c54bc7badd4 | 109 | typedef SelfTestResponse Response; |
jjzak | 6:3c54bc7badd4 | 110 | }; |
jjzak | 6:3c54bc7badd4 | 111 | |
jjzak | 6:3c54bc7badd4 | 112 | } |
jjzak | 6:3c54bc7badd4 | 113 | #endif |
jjzak | 6:3c54bc7badd4 | 114 |