From 4b329a36e03ca4872fc78a9da517dd49af0045c1 Mon Sep 17 00:00:00 2001
From: hehe <you@example.com>
Date: Mon, 5 Jan 2026 12:45:58 +0800
Subject: [PATCH] add meta_key client

---
 meta_dev/launch/meta_dev.launch.py |   4 +-
 meta_dev/src/main.cpp              | 254 ++++++++++++++++++-----------
 2 files changed, 161 insertions(+), 97 deletions(-)

diff --git a/meta_dev/launch/meta_dev.launch.py b/meta_dev/launch/meta_dev.launch.py
index f375b9e..74095d6 100644
--- a/meta_dev/launch/meta_dev.launch.py
+++ b/meta_dev/launch/meta_dev.launch.py
@@ -2,5 +2,5 @@ from launch import LaunchDescription
 from launch_ros.actions import Node
 def generate_launch_description():
     return LaunchDescription([
-        Node(package="meta_dev",executable="meta_dev_node",name="meta_dev_node",output="screen",parameters=[])
-    ])
\ No newline at end of file
+        Node(package="meta_dev",executable="meta_dev_node",name="meta_dev_node",output="screen",parameters=[{"meta_ip":"192.168.10.195"}])
+    ])
diff --git a/meta_dev/src/main.cpp b/meta_dev/src/main.cpp
index 8d40c21..1c3ba7d 100644
--- a/meta_dev/src/main.cpp
+++ b/meta_dev/src/main.cpp
@@ -1,6 +1,7 @@
 #include"rclcpp/rclcpp.hpp"
 #include"sys/socket.h"
 #include "netinet/in.h"
+#include <arpa/inet.h>
 #include "unistd.h"
 #include "interfaces/msg/meta_key.hpp"
 #include <fcntl.h>
@@ -29,104 +30,117 @@ class UdpServerNode:public rclcpp::Node
             int32_t  wr;
         };
         HandData hand_data;
+        std::string meta_ip="192.168.10.73";
+        const int meta_port=6666;
+        const int robot_port = 9999;
+        const int udp_buf_len=256;
+        unsigned char udp_buf[256];
     public:
         UdpServerNode():Node("meta_dev_node"){
             pub_meta=create_publisher<interfaces::msg::MetaKey>("/meta_key",10);
             init_crc8();
-            udp_thread=std::thread([this](){
-                struct sockaddr_in server_addr,client_addr;
-                socklen_t client_len = sizeof(client_addr);
-                const int PORT = 9999;
-                const int udp_buf_len=256;
-                unsigned char udp_buf[256];
-                int sockfd;
-                if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
-                    printf("UDP socket creation failed");
-                    return;
-                }
-                int flags = fcntl(sockfd, F_GETFL, 0);
-                fcntl(sockfd, F_SETFL, flags | SOCK_NONBLOCK);
-                // 配置服务器地址
-                memset(&server_addr, 0, sizeof(server_addr));
-                server_addr.sin_family = AF_INET;
-                server_addr.sin_addr.s_addr = INADDR_ANY;
-                server_addr.sin_port = htons(PORT);
-
-                // 绑定端口
-                if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
-                    printf("Bind failed on port %d", PORT);
-                    close(sockfd);
-                    return;
-                }
-
-                std::cout<<"UDP listener bound to port:"<< PORT<<std::endl;
-                while (rclcpp::ok()) {
-                    //std::cout<<"start recv..."<<std::endl;
-                    ssize_t ret = recvfrom(sockfd, udp_buf, udp_buf_len, 0,
-                                                    (struct sockaddr *)&client_addr, &client_len);
-                    if (ret > 0) {
-                        std::cout<<"udp recv:"<<std::endl;
-                        for(int i=0;i<ret;i++){
-                            std::cout<<std::hex<<(unsigned int)udp_buf[i]<<",";
-                        }
-                        std::cout<<std::endl;
-                        unsigned char crc=calc_crc8(udp_buf,ret-1);
-                        if(udp_buf[ret-1]!=crc)
-                            std::cout<<"crc:"<<std::hex<<(unsigned int)crc<<(unsigned int)udp_buf[ret-1]<<std::endl;
-                        hand_data.addr=udp_buf[0];
-                        hand_data.func=udp_buf[1];
-                        hand_data.a=udp_buf[2];
-                        hand_data.b=udp_buf[3];
-                        hand_data.sk=udp_buf[4];
-                        hand_data.grab=udp_buf[5];
-                        hand_data.sk_x=udp_buf[6];
-                        hand_data.sk_y=udp_buf[7];
-                        hand_data.x=udp_buf[11]<<24|udp_buf[10]<<16|udp_buf[9]<<8|udp_buf[8];
-                        hand_data.y=udp_buf[15]<<24|udp_buf[14]<<16|udp_buf[13]<<8|udp_buf[12];
-                        hand_data.z=udp_buf[19]<<24|udp_buf[18]<<16|udp_buf[17]<<8|udp_buf[16];
-                        hand_data.xr=udp_buf[23]<<24|udp_buf[22]<<16|udp_buf[21]<<8|udp_buf[20];
-                        hand_data.yr=udp_buf[27]<<24|udp_buf[26]<<16|udp_buf[25]<<8|udp_buf[24];
-                        hand_data.zr=udp_buf[31]<<24|udp_buf[30]<<16|udp_buf[29]<<8|udp_buf[28];
-                        hand_data.wr=udp_buf[35]<<24|udp_buf[34]<<16|udp_buf[33]<<8|udp_buf[32];
-                        std::cout<<"hand:"<<std::dec<<(int)hand_data.a<<","<<(int)hand_data.b<<","<<(int)hand_data.sk<<","<<(int)hand_data.grab<<","
-                                <<(int)hand_data.sk_x<<","<<(int)hand_data.sk_y<<","<<(int)hand_data.x<<","<<(int)hand_data.y<<","<<(int)hand_data.z<<","
-                                <<(int)hand_data.xr<<","<<(int)hand_data.yr<<","<<(int)hand_data.zr<<","<<(int)hand_data.wr<<std::endl;
-                        interfaces::msg::MetaKey metaKey;
-                        metaKey.source="quest3";
-                        if(udp_buf[0]==0xf0)
-                            metaKey.type="right";
-                        else if(udp_buf[0]==0xf1)
-                            metaKey.type="left";
-                        metaKey.a=hand_data.a;
-                        metaKey.b=hand_data.b;
-                        metaKey.sk=hand_data.sk;
-                        metaKey.trigger=hand_data.grab;
-                        metaKey.sk_x=hand_data.sk_x;
-                        metaKey.sk_y=hand_data.sk_y;
-                        metaKey.pose.position.x=hand_data.x/100.0f;
-                        metaKey.pose.position.y=hand_data.y/100.0f;
-                        metaKey.pose.position.z=hand_data.z/100.0f;
-
-                        metaKey.pose.orientation.x=hand_data.xr/10000.0f;
-                        metaKey.pose.orientation.y=hand_data.yr/10000.0f;
-                        metaKey.pose.orientation.z=hand_data.zr/10000.0f;
-                        metaKey.pose.orientation.w=hand_data.wr/10000.0f;
-                        this->pub_meta->publish(metaKey);
-                    } else if (ret < 0) {
-                        if (errno == EAGAIN || errno == EWOULDBLOCK ) {
-                            std::this_thread::sleep_for(std::chrono::milliseconds(10));
-                            continue;
-                        }
-                        std::cout<<"recvfrom error:"<<std::endl;
-                        break;
-                    }
-                }
-                close(sockfd);
-                std::cout<<"UDP listener stopped."<<std::endl;
-            });
+            declare_parameter("meta_ip",std::string("192.168.10.73"));
+            meta_ip=get_parameter("meta_ip").as_string();
         }
-        void pub_info(){
-            
+        void init(){
+            int ret=login_meta(1);
+            if(ret==1){
+                udp_thread=std::thread([this](){
+                    struct sockaddr_in server_addr,client_addr;
+                    socklen_t client_len = sizeof(client_addr);
+                    int sockfd;
+                    if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+                        printf("UDP socket creation failed");
+                        return;
+                    }
+                    int flags = fcntl(sockfd, F_GETFL, 0);
+                    fcntl(sockfd, F_SETFL, flags | SOCK_NONBLOCK);
+                    // 配置服务器地址
+                    memset(&server_addr, 0, sizeof(server_addr));
+                    server_addr.sin_family = AF_INET;
+                    server_addr.sin_addr.s_addr = INADDR_ANY;
+                    server_addr.sin_port = htons(robot_port);
+
+                    // 绑定端口
+                    if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
+                        printf("Bind failed on port %d", robot_port);
+                        close(sockfd);
+                        return;
+                    }
+
+                    std::cout<<"UDP listener bound to port:"<< robot_port<<std::endl;
+                    while (rclcpp::ok()) {
+                        //std::cout<<"start recv..."<<std::endl;
+                        ssize_t ret = recvfrom(sockfd, udp_buf, udp_buf_len, 0,
+                                                        (struct sockaddr *)&client_addr, &client_len);
+                        if (ret > 0) {
+                        /*
+                            std::cout<<"udp recv:"<<std::endl;
+                            for(int i=0;i<ret;i++){
+                                std::cout<<std::hex<<(unsigned int)udp_buf[i]<<",";
+                            }
+                            std::cout<<std::endl;
+                        */
+                            unsigned char crc=calc_crc8(udp_buf,ret-1);
+                            if(udp_buf[ret-1]!=crc)
+                                std::cout<<"crc:"<<std::hex<<(unsigned int)crc<<(unsigned int)udp_buf[ret-1]<<std::endl;
+                            hand_data.addr=udp_buf[0];
+                            hand_data.func=udp_buf[1];
+                            hand_data.a=udp_buf[2];
+                            hand_data.b=udp_buf[3];
+                            hand_data.sk=udp_buf[4];
+                            hand_data.grab=udp_buf[5];
+                            hand_data.sk_x=udp_buf[6];
+                            hand_data.sk_y=udp_buf[7];
+                            hand_data.x=udp_buf[11]<<24|udp_buf[10]<<16|udp_buf[9]<<8|udp_buf[8];
+                            hand_data.y=udp_buf[15]<<24|udp_buf[14]<<16|udp_buf[13]<<8|udp_buf[12];
+                            hand_data.z=udp_buf[19]<<24|udp_buf[18]<<16|udp_buf[17]<<8|udp_buf[16];
+                            hand_data.xr=udp_buf[23]<<24|udp_buf[22]<<16|udp_buf[21]<<8|udp_buf[20];
+                            hand_data.yr=udp_buf[27]<<24|udp_buf[26]<<16|udp_buf[25]<<8|udp_buf[24];
+                            hand_data.zr=udp_buf[31]<<24|udp_buf[30]<<16|udp_buf[29]<<8|udp_buf[28];
+                            hand_data.wr=udp_buf[35]<<24|udp_buf[34]<<16|udp_buf[33]<<8|udp_buf[32];
+                            /*
+                            std::cout<<"hand:"<<std::dec<<(int)hand_data.a<<","<<(int)hand_data.b<<","<<(int)hand_data.sk<<","<<(int)hand_data.grab<<","
+                                    <<(int)hand_data.sk_x<<","<<(int)hand_data.sk_y<<","<<(int)hand_data.x<<","<<(int)hand_data.y<<","<<(int)hand_data.z<<","
+                                    <<(int)hand_data.xr<<","<<(int)hand_data.yr<<","<<(int)hand_data.zr<<","<<(int)hand_data.wr<<std::endl;
+                            */
+                            interfaces::msg::MetaKey metaKey;
+                            metaKey.source="quest3";
+                            if(udp_buf[0]==0xf0)
+                                metaKey.type="right";
+                            else if(udp_buf[0]==0xf1)
+                                metaKey.type="left";
+                            metaKey.a=hand_data.a;
+                            metaKey.b=hand_data.b;
+                            metaKey.sk=hand_data.sk;
+                            metaKey.trigger=hand_data.grab;
+                            metaKey.sk_x=hand_data.sk_x;
+                            metaKey.sk_y=hand_data.sk_y;
+                            metaKey.pose.position.x=hand_data.x/10000.0f;
+                            metaKey.pose.position.y=hand_data.y/10000.0f;
+                            metaKey.pose.position.z=hand_data.z/10000.0f;
+
+                            metaKey.pose.orientation.x=hand_data.xr/10000.0f;
+                            metaKey.pose.orientation.y=hand_data.yr/10000.0f;
+                            metaKey.pose.orientation.z=hand_data.zr/10000.0f;
+                            metaKey.pose.orientation.w=hand_data.wr/10000.0f;
+                            this->pub_meta->publish(metaKey);
+                        } else if (ret < 0) {
+                            if (errno == EAGAIN || errno == EWOULDBLOCK ) {
+                                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                                continue;
+                            }
+                            std::cout<<"recvfrom error:"<<std::endl;
+                            break;
+                        }
+                    }
+                    close(sockfd);
+                    std::cout<<"UDP listener stopped."<<std::endl;
+                });
+            }else{
+                std::cout<<"login fail app exit!"<<std::endl;
+                rclcpp::shutdown();
+            }
         }
     
     void init_crc8(){
@@ -153,13 +167,63 @@ class UdpServerNode:public rclcpp::Node
         }
         return crc; 
     }
-    
+    int login_meta(int type){
+        
+        struct sockaddr_in server_addr;
+        udp_buf[0]=0xb0;
+        udp_buf[1]=type;//1--login,0--logout
+        udp_buf[2]=100;//发送间隔ms
+        udp_buf[3]=calc_crc8(udp_buf,3);
+        int sockfd;
+        if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+            printf("UDP socket creation failed");
+            return 0;
+        }
+        //int flags = fcntl(sockfd, F_GETFL, 0);
+        //fcntl(sockfd, F_SETFL, flags | SOCK_NONBLOCK);
+        memset(&server_addr, 0, sizeof(server_addr));
+        server_addr.sin_family = AF_INET;
+        server_addr.sin_port = htons(meta_port);
+        server_addr.sin_addr.s_addr=inet_addr(meta_ip.c_str());
+
+        ssize_t send_len = sendto(sockfd, udp_buf, 4, 0,(const struct sockaddr *)&server_addr, sizeof(server_addr));
+        if (send_len < 0) {
+            perror("sendto failed");
+            close(sockfd);
+            return 0;
+        }
+        struct timeval timeout;
+        timeout.tv_sec = 5;
+        timeout.tv_usec = 0;
+        if (setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) {
+            perror("setsockopt failed");
+            close(sockfd);
+            return 0;
+        }
+        socklen_t addr_len = sizeof(server_addr);
+        int n = recvfrom(sockfd, udp_buf, udp_buf_len, 0,(struct sockaddr *)&server_addr, &addr_len);
+        close(sockfd);
+        printf("sock recv:%d\n",n);
+        if (n > 0) {
+            printf("Server replied: %02x,%02x,%02x\n", udp_buf[0], udp_buf[1], udp_buf[2]);
+            return 1;
+        } else {
+            if(type==1)
+                printf("meta not response login fail \n");
+            else if(type==0)
+                printf("meta not response logout fail \n");
+            return 0;
+        }
+        
+    }
 };
 
 int main(int argc,char* argv[]){
     rclcpp::init(argc,argv);
     auto node =std::make_shared<UdpServerNode>();
+    node->init();
     rclcpp::spin(node);
+    node->login_meta(0);
     rclcpp::shutdown();
     return 0;
-}
\ No newline at end of file
+}