micro_ros_arduino/src/husarnet_transport.cpp at 9c83c2eea88b22ba46ed9562f2e5895d78514f2c · micro-ROS/micro_ros_arduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#if defined(ESP32) && defined(HUSARNET)
#include <Arduino.h>
#include <Husarnet.h>
#include <HusarnetClient.h>
#include <micro_ros_arduino.h>

extern "C" {

static HusarnetClient client;

bool arduino_husarnet_transport_open(struct uxrCustomTransport *transport) {
  struct micro_ros_agent_locator *locator =
      (struct micro_ros_agent_locator *)transport->args;

  Serial1.printf("Connecting to \"%s:%d\"... ", locator->hostname,
                 locator->port);

  /* Try to connect to a server on port 8888 on your laptop */
  if (!client.connect(locator->hostname, locator->port)) {
    Serial1.printf("failed\r\n");
    return false;
  }

  Serial1.printf("done\r\n");

  return true;
}

bool arduino_husarnet_transport_close(struct uxrCustomTransport *transport) {
  client.stop();
  return true;
}

size_t arduino_husarnet_transport_write(struct uxrCustomTransport *transport,
                                        const uint8_t *buf, size_t len,
                                        uint8_t *errcode) {
  (void)errcode;

  // As we are using a TCP stream connection we should indicate the size of the message with the first two bytes of the stream.
  static uint8_t buffer_size[2];
  buffer_size[0] = (uint8_t)(0x00FF & len);
  buffer_size[1] = (uint8_t)((0xFF00 & len) >> 8);
  size_t sent = client.write(buffer_size, 2);

  // Then we send the payload
  if (sent == 2) {
    sent = client.write(buf, len);
  } else {
    sent = 0;
  }

  return sent;
}

// Sample state machine for receiving data
typedef enum {
  STATE_WAIT_FOR_SIZE = 0,
  STATE_WAIT_FOR_DATA,
  STATE_MESSAGE_AVAILABLE
} husarnet_tcp_states_t;

typedef struct {
  uint8_t buffer[UXR_CONFIG_CUSTOM_TRANSPORT_MTU];

  uint8_t length_buffer[2];

  uint16_t message_size;
  uint16_t message_size_received;

  husarnet_tcp_states_t state;
} husarnet_tcp_receiver_t;

static husarnet_tcp_receiver_t receiver = {};

void read_tcp_data(husarnet_tcp_receiver_t & r) {
  switch(r.state) {
    case STATE_WAIT_FOR_SIZE:
      if (client.available() >= 2)
      {
        client.read(r.length_buffer, 2);
        r.message_size = (r.length_buffer[0] | (r.length_buffer[1] << 8));
        r.message_size_received = 0;
        r.state = STATE_WAIT_FOR_DATA;
      }
      break;
    case STATE_WAIT_FOR_DATA:
      if(client.available())
      {
        size_t to_read = (r.message_size - r.message_size_received) < client.available() ? r.message_size - r.message_size_received : client.available();
        size_t readed = client.read(&r.buffer[r.message_size_received], to_read);
        r.message_size_received += readed;
        if(r.message_size_received == r.message_size){
          r.state = STATE_MESSAGE_AVAILABLE;
        }
      }
      break;
    case STATE_MESSAGE_AVAILABLE:
      break;
  }
}

size_t arduino_husarnet_transport_read(struct uxrCustomTransport *transport,
                                       uint8_t *buf, size_t len, int timeout,
                                       uint8_t *errcode) {
  (void)errcode;

  client.setTimeout(timeout);

  do
  {
    int64_t time_init = uxr_millis();
    read_tcp_data(receiver);
    timeout -= (int)(uxr_millis() - time_init);
  }
  while ((STATE_MESSAGE_AVAILABLE != receiver.state) && (0 < timeout));

  if (STATE_MESSAGE_AVAILABLE == receiver.state)
  {
    size_t readed = receiver.message_size;
    memcpy(buf, receiver.buffer, readed);
    receiver.state = STATE_WAIT_FOR_SIZE;
    return readed;
  }
  else
  {
    return 0;
  }
}

}

#endif