nucho / rosserial_mbed_lib

This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.

Dependencies:  

Dependents:   rosserial_mbed robot_S2

dianostic_msgs/DiagnosticStatus.h@1:ff0ec969dad1, 2011-10-16 (annotated)

Committer:
nucho
Date:
Sun Oct 16 07:19:36 2011 +0000
Revision:
1:ff0ec969dad1
Parent:
0:77afd7560544
Child:
3:1cf99502f396
This program supported the revision of 143 of rosserial.

And the bug fix of receive of array data.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nucho 1:ff0ec969dad1 1 #ifndef ros_diagnostic_msgs_DiagnosticStatus_h
nucho 1:ff0ec969dad1 2 #define ros_diagnostic_msgs_DiagnosticStatus_h
nucho 0:77afd7560544 3
nucho 0:77afd7560544 4 #include <stdint.h>
nucho 0:77afd7560544 5 #include <string.h>
nucho 0:77afd7560544 6 #include <stdlib.h>
nucho 0:77afd7560544 7 #include "../ros/msg.h"
nucho 0:77afd7560544 8 #include "diagnostic_msgs/KeyValue.h"
nucho 0:77afd7560544 9
nucho 1:ff0ec969dad1 10 namespace diagnostic_msgs {
nucho 0:77afd7560544 11
nucho 1:ff0ec969dad1 12 class DiagnosticStatus : public ros::Msg {
nucho 1:ff0ec969dad1 13 public:
nucho 1:ff0ec969dad1 14 unsigned char level;
nucho 1:ff0ec969dad1 15 char * name;
nucho 1:ff0ec969dad1 16 char * message;
nucho 1:ff0ec969dad1 17 char * hardware_id;
nucho 1:ff0ec969dad1 18 unsigned char values_length;
nucho 1:ff0ec969dad1 19 diagnostic_msgs::KeyValue st_values;
nucho 1:ff0ec969dad1 20 diagnostic_msgs::KeyValue * values;
nucho 1:ff0ec969dad1 21 enum { OK = 0 };
nucho 1:ff0ec969dad1 22 enum { WARN = 1 };
nucho 1:ff0ec969dad1 23 enum { ERROR = 2 };
nucho 0:77afd7560544 24
nucho 1:ff0ec969dad1 25 virtual int serialize(unsigned char *outbuffer) {
nucho 1:ff0ec969dad1 26 int offset = 0;
nucho 1:ff0ec969dad1 27 union {
nucho 1:ff0ec969dad1 28 unsigned char real;
nucho 1:ff0ec969dad1 29 unsigned char base;
nucho 1:ff0ec969dad1 30 } u_level;
nucho 1:ff0ec969dad1 31 u_level.real = this->level;
nucho 1:ff0ec969dad1 32 *(outbuffer + offset + 0) = (u_level.base >> (8 * 0)) & 0xFF;
nucho 1:ff0ec969dad1 33 offset += sizeof(this->level);
nucho 1:ff0ec969dad1 34 long * length_name = (long *)(outbuffer + offset);
nucho 1:ff0ec969dad1 35 *length_name = strlen( (const char*) this->name);
nucho 1:ff0ec969dad1 36 offset += 4;
nucho 1:ff0ec969dad1 37 memcpy(outbuffer + offset, this->name, *length_name);
nucho 1:ff0ec969dad1 38 offset += *length_name;
nucho 1:ff0ec969dad1 39 long * length_message = (long *)(outbuffer + offset);
nucho 1:ff0ec969dad1 40 *length_message = strlen( (const char*) this->message);
nucho 1:ff0ec969dad1 41 offset += 4;
nucho 1:ff0ec969dad1 42 memcpy(outbuffer + offset, this->message, *length_message);
nucho 1:ff0ec969dad1 43 offset += *length_message;
nucho 1:ff0ec969dad1 44 long * length_hardware_id = (long *)(outbuffer + offset);
nucho 1:ff0ec969dad1 45 *length_hardware_id = strlen( (const char*) this->hardware_id);
nucho 1:ff0ec969dad1 46 offset += 4;
nucho 1:ff0ec969dad1 47 memcpy(outbuffer + offset, this->hardware_id, *length_hardware_id);
nucho 1:ff0ec969dad1 48 offset += *length_hardware_id;
nucho 1:ff0ec969dad1 49 *(outbuffer + offset++) = values_length;
nucho 1:ff0ec969dad1 50 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 51 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 52 *(outbuffer + offset++) = 0;
nucho 1:ff0ec969dad1 53 for ( unsigned char i = 0; i < values_length; i++) {
nucho 1:ff0ec969dad1 54 offset += this->values[i].serialize(outbuffer + offset);
nucho 1:ff0ec969dad1 55 }
nucho 1:ff0ec969dad1 56 return offset;
nucho 0:77afd7560544 57 }
nucho 0:77afd7560544 58
nucho 1:ff0ec969dad1 59 virtual int deserialize(unsigned char *inbuffer) {
nucho 1:ff0ec969dad1 60 int offset = 0;
nucho 1:ff0ec969dad1 61 union {
nucho 1:ff0ec969dad1 62 unsigned char real;
nucho 1:ff0ec969dad1 63 unsigned char base;
nucho 1:ff0ec969dad1 64 } u_level;
nucho 1:ff0ec969dad1 65 u_level.base = 0;
nucho 1:ff0ec969dad1 66 u_level.base |= ((typeof(u_level.base)) (*(inbuffer + offset + 0))) << (8 * 0);
nucho 1:ff0ec969dad1 67 this->level = u_level.real;
nucho 1:ff0ec969dad1 68 offset += sizeof(this->level);
nucho 1:ff0ec969dad1 69 uint32_t length_name = *(uint32_t *)(inbuffer + offset);
nucho 1:ff0ec969dad1 70 offset += 4;
nucho 1:ff0ec969dad1 71 for (unsigned int k= offset; k< offset+length_name; ++k) {
nucho 1:ff0ec969dad1 72 inbuffer[k-1]=inbuffer[k];
nucho 1:ff0ec969dad1 73 }
nucho 1:ff0ec969dad1 74 inbuffer[offset+length_name-1]=0;
nucho 1:ff0ec969dad1 75 this->name = (char *)(inbuffer + offset-1);
nucho 1:ff0ec969dad1 76 offset += length_name;
nucho 1:ff0ec969dad1 77 uint32_t length_message = *(uint32_t *)(inbuffer + offset);
nucho 1:ff0ec969dad1 78 offset += 4;
nucho 1:ff0ec969dad1 79 for (unsigned int k= offset; k< offset+length_message; ++k) {
nucho 1:ff0ec969dad1 80 inbuffer[k-1]=inbuffer[k];
nucho 1:ff0ec969dad1 81 }
nucho 1:ff0ec969dad1 82 inbuffer[offset+length_message-1]=0;
nucho 1:ff0ec969dad1 83 this->message = (char *)(inbuffer + offset-1);
nucho 1:ff0ec969dad1 84 offset += length_message;
nucho 1:ff0ec969dad1 85 uint32_t length_hardware_id = *(uint32_t *)(inbuffer + offset);
nucho 1:ff0ec969dad1 86 offset += 4;
nucho 1:ff0ec969dad1 87 for (unsigned int k= offset; k< offset+length_hardware_id; ++k) {
nucho 1:ff0ec969dad1 88 inbuffer[k-1]=inbuffer[k];
nucho 1:ff0ec969dad1 89 }
nucho 1:ff0ec969dad1 90 inbuffer[offset+length_hardware_id-1]=0;
nucho 1:ff0ec969dad1 91 this->hardware_id = (char *)(inbuffer + offset-1);
nucho 1:ff0ec969dad1 92 offset += length_hardware_id;
nucho 1:ff0ec969dad1 93 unsigned char values_lengthT = *(inbuffer + offset++);
nucho 1:ff0ec969dad1 94 if (values_lengthT > values_length)
nucho 1:ff0ec969dad1 95 this->values = (diagnostic_msgs::KeyValue*)realloc(this->values, values_lengthT * sizeof(diagnostic_msgs::KeyValue));
nucho 1:ff0ec969dad1 96 offset += 3;
nucho 1:ff0ec969dad1 97 values_length = values_lengthT;
nucho 1:ff0ec969dad1 98 for ( unsigned char i = 0; i < values_length; i++) {
nucho 1:ff0ec969dad1 99 offset += this->st_values.deserialize(inbuffer + offset);
nucho 1:ff0ec969dad1 100 memcpy( &(this->values[i]), &(this->st_values), sizeof(diagnostic_msgs::KeyValue));
nucho 1:ff0ec969dad1 101 }
nucho 1:ff0ec969dad1 102 return offset;
nucho 0:77afd7560544 103 }
nucho 0:77afd7560544 104
nucho 1:ff0ec969dad1 105 virtual const char * getType() {
nucho 1:ff0ec969dad1 106 return "diagnostic_msgs/DiagnosticStatus";
nucho 1:ff0ec969dad1 107 };
nucho 0:77afd7560544 108
nucho 1:ff0ec969dad1 109 };
nucho 0:77afd7560544 110
nucho 0:77afd7560544 111 }
nucho 0:77afd7560544 112 #endif