communication/send_info.cpp at master · dreamer198/communication · 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
#include <iostream>
#include <vector>
#include <string>
#include <cstring>      // for memset
#include <arpa/inet.h>  // for inet_addr, sockaddr_in (on Linux)
#include <sys/socket.h> // for socket functions
#include <unistd.h>     // for close()

struct UAV_info {
    float pos[3] = {0, 0, 0};
    float q[4] = {1, 0, 0, 0};
    float battery = 0;
};

// 发送信息（组装字符串并广播）
void send_info(const UAV_info& info, int port) {
    std::string head = "state";
    std::string message = head + " " +
                          std::to_string(info.pos[0]) + " " + std::to_string(info.pos[1]) + " " + std::to_string(info.pos[2]) + " " +
                          std::to_string(info.q[0]) + " " + std::to_string(info.q[1]) + " " + std::to_string(info.q[2]) + " " +
                          std::to_string(info.q[3]) + " " +
                          std::to_string(info.battery);

    // 发送广播
    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (sock < 0) {
        std::cerr << "创建UDP socket失败！\n";
        return;
    }

    // 设置 socket 为可广播
    int broadcastEnable = 1;
    if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, (char *)&broadcastEnable, sizeof(broadcastEnable)) < 0) {
        std::cerr << "设置广播选项失败！\n";
        close(sock);
        return;
    }

    sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    inet_pton(AF_INET, "192.168.1.255", &(addr.sin_addr));
    // addr.sin_addr.s_addr = inet_addr("10.101.121.255"); // 通信地址地址

    ssize_t sent = sendto(sock, message.c_str(), message.size(), 0,
                          reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
    if (sent < 0) {
        std::cerr << "广播发送失败！\n";
    } else {
        std::cout << "已广播指令 \"" << message << "\" 到端口 " << port << "\n";
    }

    close(sock);
}

int main() {
    UAV_info uav{{1, 2, -3}, {0, 1, 0, 0}, 50};
    int port = 10003;
    send_info(uav, port);
    return 0;
}


// #include <iostream>
// #include <string>
// #include <cstring>
// #include <arpa/inet.h>
// #include <sys/socket.h>
// #include <unistd.h>
// #include <netinet/in.h>  // 为了跨平台统一包含

// struct UAV_info {
//     float pos[3] = {0, 0, 0};
//     float q[4] = {1, 0, 0, 0};
//     float battery = 0;
// };

// // 发送信息（组装字符串并广播）
// void send_info(const UAV_info& info, int port) {
//     // 构造消息字符串
//     std::string message = "state " +
//         std::to_string(info.pos[0]) + " " + std::to_string(info.pos[1]) + " " + std::to_string(info.pos[2]) + " " +
//         std::to_string(info.q[0]) + " " + std::to_string(info.q[1]) + " " + std::to_string(info.q[2]) + " " +
//         std::to_string(info.q[3]) + " " + std::to_string(info.battery);

//     // 创建UDP socket
//     int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
//     if (sock < 0) {
//         perror("创建 socket 失败");
//         return;
//     }

//     // 设置 socket 允许广播
//     int broadcastEnable = 1;
//     if (setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &broadcastEnable, sizeof(broadcastEnable)) < 0) {
//         perror("设置 SO_BROADCAST 失败");
//         close(sock);
//         return;
//     }

//     // 填充目标地址结构体
//     struct sockaddr_in broadcastAddr;
//     memset(&broadcastAddr, 0, sizeof(broadcastAddr));
//     broadcastAddr.sin_family = AF_INET;
//     broadcastAddr.sin_port = htons(port);
//     broadcastAddr.sin_addr.s_addr = inet_addr("10.101.121.255");  // 替换为你网段的广播地址

//     // 发送广播消息
//     ssize_t sentBytes = sendto(sock, message.c_str(), message.size(), 0,
//                                reinterpret_cast<struct sockaddr*>(&broadcastAddr), sizeof(broadcastAddr));

//     if (sentBytes < 0) {
//         perror("广播发送失败");
//     } else {
//         std::cout << "成功广播：" << message << " 到端口 " << port << std::endl;
//     }

//     close(sock);
// }

// int main() {
//     UAV_info uav{{0, 0, 1}, {1, 0, 0, 0}, 0.5f};
//     int port = 10016;
//     send_info(uav, port);
//     return 0;
// }