Soft robot team / ros_lib_kinetic

Dependencies:   BufferedSerial

mbed_custom_msgs/lidar_msg.h@2:04b84e97cabd, 2019-10-30 (annotated)

Committer:
florine_van
Date:
Wed Oct 30 13:12:15 2019 +0000
Revision:
2:04b84e97cabd
Add custom message;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 2:04b84e97cabd 1 #ifndef _ROS_mbed_custom_msgs_lidar_msg_h
florine_van 2:04b84e97cabd 2 #define _ROS_mbed_custom_msgs_lidar_msg_h
florine_van 2:04b84e97cabd 3
florine_van 2:04b84e97cabd 4 #include <stdint.h>
florine_van 2:04b84e97cabd 5 #include <string.h>
florine_van 2:04b84e97cabd 6 #include <stdlib.h>
florine_van 2:04b84e97cabd 7 #include "ros/msg.h"
florine_van 2:04b84e97cabd 8 #include "std_msgs/Header.h"
florine_van 2:04b84e97cabd 9
florine_van 2:04b84e97cabd 10 namespace mbed_custom_msgs
florine_van 2:04b84e97cabd 11 {
florine_van 2:04b84e97cabd 12
florine_van 2:04b84e97cabd 13 class lidar_msg : public ros::Msg
florine_van 2:04b84e97cabd 14 {
florine_van 2:04b84e97cabd 15 public:
florine_van 2:04b84e97cabd 16 typedef std_msgs::Header _header_type;
florine_van 2:04b84e97cabd 17 _header_type header;
florine_van 2:04b84e97cabd 18 typedef uint32_t _sensor_forward_type;
florine_van 2:04b84e97cabd 19 _sensor_forward_type sensor_forward;
florine_van 2:04b84e97cabd 20 typedef uint32_t _sensor_back_type;
florine_van 2:04b84e97cabd 21 _sensor_back_type sensor_back;
florine_van 2:04b84e97cabd 22 typedef uint32_t _sensor_left_type;
florine_van 2:04b84e97cabd 23 _sensor_left_type sensor_left;
florine_van 2:04b84e97cabd 24 typedef uint32_t _sensor_right_type;
florine_van 2:04b84e97cabd 25 _sensor_right_type sensor_right;
florine_van 2:04b84e97cabd 26 typedef const char* _data_type;
florine_van 2:04b84e97cabd 27 _data_type data;
florine_van 2:04b84e97cabd 28
florine_van 2:04b84e97cabd 29 lidar_msg():
florine_van 2:04b84e97cabd 30 header(),
florine_van 2:04b84e97cabd 31 sensor_forward(0),
florine_van 2:04b84e97cabd 32 sensor_back(0),
florine_van 2:04b84e97cabd 33 sensor_left(0),
florine_van 2:04b84e97cabd 34 sensor_right(0),
florine_van 2:04b84e97cabd 35 data("")
florine_van 2:04b84e97cabd 36 {
florine_van 2:04b84e97cabd 37 }
florine_van 2:04b84e97cabd 38
florine_van 2:04b84e97cabd 39 virtual int serialize(unsigned char *outbuffer) const
florine_van 2:04b84e97cabd 40 {
florine_van 2:04b84e97cabd 41 int offset = 0;
florine_van 2:04b84e97cabd 42 offset += this->header.serialize(outbuffer + offset);
florine_van 2:04b84e97cabd 43 *(outbuffer + offset + 0) = (this->sensor_forward >> (8 * 0)) & 0xFF;
florine_van 2:04b84e97cabd 44 *(outbuffer + offset + 1) = (this->sensor_forward >> (8 * 1)) & 0xFF;
florine_van 2:04b84e97cabd 45 *(outbuffer + offset + 2) = (this->sensor_forward >> (8 * 2)) & 0xFF;
florine_van 2:04b84e97cabd 46 *(outbuffer + offset + 3) = (this->sensor_forward >> (8 * 3)) & 0xFF;
florine_van 2:04b84e97cabd 47 offset += sizeof(this->sensor_forward);
florine_van 2:04b84e97cabd 48 *(outbuffer + offset + 0) = (this->sensor_back >> (8 * 0)) & 0xFF;
florine_van 2:04b84e97cabd 49 *(outbuffer + offset + 1) = (this->sensor_back >> (8 * 1)) & 0xFF;
florine_van 2:04b84e97cabd 50 *(outbuffer + offset + 2) = (this->sensor_back >> (8 * 2)) & 0xFF;
florine_van 2:04b84e97cabd 51 *(outbuffer + offset + 3) = (this->sensor_back >> (8 * 3)) & 0xFF;
florine_van 2:04b84e97cabd 52 offset += sizeof(this->sensor_back);
florine_van 2:04b84e97cabd 53 *(outbuffer + offset + 0) = (this->sensor_left >> (8 * 0)) & 0xFF;
florine_van 2:04b84e97cabd 54 *(outbuffer + offset + 1) = (this->sensor_left >> (8 * 1)) & 0xFF;
florine_van 2:04b84e97cabd 55 *(outbuffer + offset + 2) = (this->sensor_left >> (8 * 2)) & 0xFF;
florine_van 2:04b84e97cabd 56 *(outbuffer + offset + 3) = (this->sensor_left >> (8 * 3)) & 0xFF;
florine_van 2:04b84e97cabd 57 offset += sizeof(this->sensor_left);
florine_van 2:04b84e97cabd 58 *(outbuffer + offset + 0) = (this->sensor_right >> (8 * 0)) & 0xFF;
florine_van 2:04b84e97cabd 59 *(outbuffer + offset + 1) = (this->sensor_right >> (8 * 1)) & 0xFF;
florine_van 2:04b84e97cabd 60 *(outbuffer + offset + 2) = (this->sensor_right >> (8 * 2)) & 0xFF;
florine_van 2:04b84e97cabd 61 *(outbuffer + offset + 3) = (this->sensor_right >> (8 * 3)) & 0xFF;
florine_van 2:04b84e97cabd 62 offset += sizeof(this->sensor_right);
florine_van 2:04b84e97cabd 63 uint32_t length_data = strlen(this->data);
florine_van 2:04b84e97cabd 64 varToArr(outbuffer + offset, length_data);
florine_van 2:04b84e97cabd 65 offset += 4;
florine_van 2:04b84e97cabd 66 memcpy(outbuffer + offset, this->data, length_data);
florine_van 2:04b84e97cabd 67 offset += length_data;
florine_van 2:04b84e97cabd 68 return offset;
florine_van 2:04b84e97cabd 69 }
florine_van 2:04b84e97cabd 70
florine_van 2:04b84e97cabd 71 virtual int deserialize(unsigned char *inbuffer)
florine_van 2:04b84e97cabd 72 {
florine_van 2:04b84e97cabd 73 int offset = 0;
florine_van 2:04b84e97cabd 74 offset += this->header.deserialize(inbuffer + offset);
florine_van 2:04b84e97cabd 75 this->sensor_forward = ((uint32_t) (*(inbuffer + offset)));
florine_van 2:04b84e97cabd 76 this->sensor_forward |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
florine_van 2:04b84e97cabd 77 this->sensor_forward |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
florine_van 2:04b84e97cabd 78 this->sensor_forward |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
florine_van 2:04b84e97cabd 79 offset += sizeof(this->sensor_forward);
florine_van 2:04b84e97cabd 80 this->sensor_back = ((uint32_t) (*(inbuffer + offset)));
florine_van 2:04b84e97cabd 81 this->sensor_back |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
florine_van 2:04b84e97cabd 82 this->sensor_back |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
florine_van 2:04b84e97cabd 83 this->sensor_back |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
florine_van 2:04b84e97cabd 84 offset += sizeof(this->sensor_back);
florine_van 2:04b84e97cabd 85 this->sensor_left = ((uint32_t) (*(inbuffer + offset)));
florine_van 2:04b84e97cabd 86 this->sensor_left |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
florine_van 2:04b84e97cabd 87 this->sensor_left |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
florine_van 2:04b84e97cabd 88 this->sensor_left |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
florine_van 2:04b84e97cabd 89 offset += sizeof(this->sensor_left);
florine_van 2:04b84e97cabd 90 this->sensor_right = ((uint32_t) (*(inbuffer + offset)));
florine_van 2:04b84e97cabd 91 this->sensor_right |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
florine_van 2:04b84e97cabd 92 this->sensor_right |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
florine_van 2:04b84e97cabd 93 this->sensor_right |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
florine_van 2:04b84e97cabd 94 offset += sizeof(this->sensor_right);
florine_van 2:04b84e97cabd 95 uint32_t length_data;
florine_van 2:04b84e97cabd 96 arrToVar(length_data, (inbuffer + offset));
florine_van 2:04b84e97cabd 97 offset += 4;
florine_van 2:04b84e97cabd 98 for(unsigned int k= offset; k< offset+length_data; ++k){
florine_van 2:04b84e97cabd 99 inbuffer[k-1]=inbuffer[k];
florine_van 2:04b84e97cabd 100 }
florine_van 2:04b84e97cabd 101 inbuffer[offset+length_data-1]=0;
florine_van 2:04b84e97cabd 102 this->data = (char *)(inbuffer + offset-1);
florine_van 2:04b84e97cabd 103 offset += length_data;
florine_van 2:04b84e97cabd 104 return offset;
florine_van 2:04b84e97cabd 105 }
florine_van 2:04b84e97cabd 106
florine_van 2:04b84e97cabd 107 const char * getType(){ return "mbed_custom_msgs/lidar_msg"; };
florine_van 2:04b84e97cabd 108 const char * getMD5(){ return "1576a60ced5a1f58d271af7970886f43"; };
florine_van 2:04b84e97cabd 109
florine_van 2:04b84e97cabd 110 };
florine_van 2:04b84e97cabd 111
florine_van 2:04b84e97cabd 112 }
florine_van 2:04b84e97cabd 113 #endif