rt-rosserial/port/RTTTcpHardware.h at master · wuhanstudio/rt-rosserial · 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
#ifndef ROS_RTT_TCP_HARDWARE_H_
#define ROS_RTT_TCP_HARDWARE_H_

#include <rtthread.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <netdb.h>

#ifndef ROSSERIAL_TCP_RECV_TIMEOUT
    #define ROSSERIAL_TCP_RECV_TIMEOUT 200000
#endif

#ifndef ROSSERIAL_TCP_SEND_TIMEOUT
    #define ROSSERIAL_TCP_SEND_TIMEOUT 200000
#endif

class RTTTcpHardware {
public:
    RTTTcpHardware()
    {
        server_ = "127.0.0.1";
        serverPort_ = 11411;
    }

    void setConnection(const char* url, int port = 11411)
    {
        server_ = url;
        serverPort_ = port;
    }

    void init() {
        struct hostent *host;
        struct sockaddr_in server_addr;
        host = gethostbyname(this->server_);

        if ((this->sock_ = socket(AF_INET, SOCK_STREAM, 0)) == -1)
        {
            rt_kprintf("[rosserial] Socket error\n");
            return;
        }

        server_addr.sin_family = AF_INET;
        server_addr.sin_port = htons(this->serverPort_);
        server_addr.sin_addr = *((struct in_addr *)host->h_addr);
        rt_memset(&(server_addr.sin_zero), 0, sizeof(server_addr.sin_zero));

        // Receive timeout
        struct timeval recv_timeout;
        recv_timeout.tv_sec =    0;
        recv_timeout.tv_usec = ROSSERIAL_TCP_RECV_TIMEOUT;
        setsockopt(this->sock_, SOL_SOCKET, SO_RCVTIMEO,
                  (void *) &recv_timeout, sizeof(recv_timeout));

        // Send timeout
        struct timeval send_timeout;
        send_timeout.tv_sec =    0;
        send_timeout.tv_usec = ROSSERIAL_TCP_SEND_TIMEOUT;
        setsockopt(this->sock_, SOL_SOCKET, SO_SNDTIMEO,
                  (void *) &send_timeout, sizeof(send_timeout));

        if (connect(this->sock_, (struct sockaddr *)&server_addr, sizeof(struct sockaddr)) == -1)
        {
            rt_kprintf("[rosserial] Connect fail!\n");
            closesocket(this->sock_);
            return;
        }
        rt_kprintf("[rosserial] Connect successful\n");
    }

    int read() {
        char ch[2];
        int bytes_received = recv(this->sock_, ch, 1, 0);
        if(bytes_received > 0)
        {
            return ch[0];
        }
        else
        {
            return -1;
        }
    }

    void write(const uint8_t* data, int length) {
        send(this->sock_, data, length, 0);
    }

    unsigned long time() {
        return ((unsigned long)rt_tick_get() * 1000 / RT_TICK_PER_SECOND);
    }

protected:
    const char *server_;
    rt_uint16_t serverPort_;
    int sock_;
};

#endif