root / scout_avr / src / ros_lib / std_msgs / Float64MultiArray.h @ 88fb3a79
History | View | Annotate | Download (2.72 KB)
1 |
#ifndef _ROS_std_msgs_Float64MultiArray_h
|
---|---|
2 |
#define _ROS_std_msgs_Float64MultiArray_h
|
3 |
|
4 |
#include <stdint.h> |
5 |
#include <string.h> |
6 |
#include <stdlib.h> |
7 |
#include "ros/msg.h" |
8 |
#include "std_msgs/MultiArrayLayout.h" |
9 |
|
10 |
namespace std_msgs |
11 |
{ |
12 |
|
13 |
class Float64MultiArray : public ros::Msg |
14 |
{ |
15 |
public:
|
16 |
std_msgs::MultiArrayLayout layout; |
17 |
uint8_t data_length; |
18 |
float st_data;
|
19 |
float * data;
|
20 |
|
21 |
virtual int serialize(unsigned char *outbuffer) const |
22 |
{ |
23 |
int offset = 0; |
24 |
offset += this->layout.serialize(outbuffer + offset); |
25 |
*(outbuffer + offset++) = data_length; |
26 |
*(outbuffer + offset++) = 0;
|
27 |
*(outbuffer + offset++) = 0;
|
28 |
*(outbuffer + offset++) = 0;
|
29 |
for( uint8_t i = 0; i < data_length; i++){ |
30 |
int32_t * val_datai = (long *) &(this->data[i]);
|
31 |
int32_t exp_datai = (((*val_datai)>>23)&255); |
32 |
if(exp_datai != 0) |
33 |
exp_datai += 1023-127; |
34 |
int32_t sig_datai = *val_datai; |
35 |
*(outbuffer + offset++) = 0;
|
36 |
*(outbuffer + offset++) = 0;
|
37 |
*(outbuffer + offset++) = 0;
|
38 |
*(outbuffer + offset++) = (sig_datai<<5) & 0xff; |
39 |
*(outbuffer + offset++) = (sig_datai>>3) & 0xff; |
40 |
*(outbuffer + offset++) = (sig_datai>>11) & 0xff; |
41 |
*(outbuffer + offset++) = ((exp_datai<<4) & 0xF0) | ((sig_datai>>19)&0x0F); |
42 |
*(outbuffer + offset++) = (exp_datai>>4) & 0x7F; |
43 |
if(this->data[i] < 0) *(outbuffer + offset -1) |= 0x80; |
44 |
} |
45 |
return offset;
|
46 |
} |
47 |
|
48 |
virtual int deserialize(unsigned char *inbuffer) |
49 |
{ |
50 |
int offset = 0; |
51 |
offset += this->layout.deserialize(inbuffer + offset); |
52 |
uint8_t data_lengthT = *(inbuffer + offset++); |
53 |
if(data_lengthT > data_length)
|
54 |
this->data = (float*)realloc(this->data, data_lengthT * sizeof(float)); |
55 |
offset += 3;
|
56 |
data_length = data_lengthT; |
57 |
for( uint8_t i = 0; i < data_length; i++){ |
58 |
uint32_t * val_st_data = (uint32_t*) &(this->st_data); |
59 |
offset += 3;
|
60 |
*val_st_data = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07); |
61 |
*val_st_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3; |
62 |
*val_st_data |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11; |
63 |
*val_st_data |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19; |
64 |
uint32_t exp_st_data = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4; |
65 |
exp_st_data |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4; |
66 |
if(exp_st_data !=0) |
67 |
*val_st_data |= ((exp_st_data)-1023+127)<<23; |
68 |
if( ((*(inbuffer+offset++)) & 0x80) > 0) this->st_data = -this->st_data; |
69 |
memcpy( &(this->data[i]), &(this->st_data), sizeof(float)); |
70 |
} |
71 |
return offset;
|
72 |
} |
73 |
|
74 |
const char * getType(){ return "std_msgs/Float64MultiArray"; }; |
75 |
const char * getMD5(){ return "4b7d974086d4060e7db4613a7e6c3ba4"; }; |
76 |
|
77 |
}; |
78 |
|
79 |
} |
80 |
#endif
|