python socket ros 多线程通信

python socket ros 多线程通信

采集数据时，鉴于ros之间的通信需要建立主从节点，对于多台ros之间的通信暂时还没找到好的解决方式，故而采用socket进行通信，相机作为服务端，开多线程，接收来自客户端（ros系统）的信息。

客户端

客户端的代码比较简单，涉及订阅相应节点，并将数据发送给服务端：

// 建立一个接听类，用于接收callback返回的数据
class Listener {
private:
    char topic_message[256];
public:
    void init();
    char* getMessageValue();
    void socketCallback(const geometry_msgs::PoseStampedConstPtr& amcl_msg);
};
char* Listener::getMessageValue(){
    return topic_message;
}

//切记对要返回的变量进行初始化
void Listener::init(){
    memset(topic_message, 0, sizeof(topic_message));
}

void Listener::socketCallback(const geometry_msgs::PoseStampedConstPtr& amcl_msg){
    syn_amcl_msg = *amcl_msg;
    memset(topic_message, 0, 256);
        
    std::string car_id = "id2:";
    std::string car_pose_x = std::to_string(syn_amcl_msg.pose.position.x);
    std::string car_pose_y = std::to_string(syn_amcl_msg.pose.position.y);
    std::string car_orient_x = std::to_string(syn_amcl_msg.pose.orientation.x);
    std::string car_orient_y = std::to_string(syn_amcl_msg.pose.orientation.y);
    std::string car_orient_z = std::to_string(syn_amcl_msg.pose.orientation.z);
    std::string car_orient_w = std::to_string(syn_amcl_msg.pose.orientation.w);
    
    std::string car = car_id + " " + car_pose_x + " " + car_pose_y + " " + car_orient_x + " " + car_orient_y + " " + car_orient_z + " " + car_orient_w;
    
    char* temp=(char*)car.data();】
    
    strcpy(topic_message, temp);
    std::cout << topic_message << std::endl;
    memset(temp,0,sizeof(temp));
};


int main(int argc, char** argv) {
    ros::init(argc, argv, "socket_node");
    ros::NodeHandle nh;
    ROS_INFO("start socket");
    ros::Rate rate(10);
    Listener listener;
    listener.init();
    //此处listener的callback函数
    ros::Subscriber sub = nh.subscribe("/amcl_pose", 1, &Listener::socketCallback, &listener);
    
    int client = socket(AF_INET, SOCK_STREAM, 0);
    if (client == -1) {
        std::cout << "Error: socket" << std::endl;
        // return 0;
    }
    struct sockaddr_in serverAddr;
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_port = htons(8888);
    serverAddr.sin_addr.s_addr = inet_addr("192.168.1.239");
    if (connect(client, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
        std::cout << "Error: connect" << std::endl;
        // return 0;
    }
    std::cout << "...connect" << std::endl;
    
    char buffer[256];
    while(ros::ok()){
    	//先进行spinOnce, 更新listener的内容，spinOnce可以在更新后继续执行后续代码  
        ros::spinOnce();      
        bzero(buffer,256);
      
        strcpy(buffer, listener.getMessageValue());
        std::cout << buffer << std::endl;
   
        send(client, buffer, sizeof(buffer), 0);
        rate.sleep();
    }
    
    close(client);
    return 0;

}

服务端

服务端是相机，在拍照的同时要保证能接收多个客户端的消息，保证照片信息与客户端信息一致，部分代码如下（相机拍照部分代码未贴）：

def clientMsg(client):
    result = ""
    while True:
        data = client.recv(1024)[0:62]
        // if判断是避免获取到一开始的空数据，并保证留下1条有用数据
        if(data and data.decode("ascii")[0] == "b" or data.decode("ascii")[0] == "r"):
            result = data.decode("ascii")
                # print(result)
            break
    return result

//创建线程类，用于接收多线程通信的返回结果
class MyThread(Thread):
    def __init__(self, func, args):
        super(MyThread, self).__init__()
        self.func = func
        self.args = args

    def run(self):
        self.result = self.func(*self.args)

    def get_result(self):
        # return self.result
        try:
            return self.result
        except Exception:
            return None

//以下是相机获取照片的代码中，与socket通信相关的部分：
def getImg():
	//获取照片内容省略...
	s = socket.socket() 
	s.bind((host, port))
	s.listen(5) 
	// 创建多个client，用于接收客户端信息
	c1, addr = s.accept()
	c2, addr = s.accept()
	c3, addr = s.accept()
	
	thlist = []
	while not rospy.is_shutdown():
	    data = ""
	    //多线程接收客户端信息，这里代码有待提高，想用for循环来着，但是验证失败，后续再优化
	    thread1 = MyThread(clientMsg, args=(c1, ))
	    thread2 = MyThread(clientMsg, args=(c2, ))
	    thread3 = MyThread(clientMsg, args=(c3, ))
	    thlist.append(thread1)
	    thlist.append(thread2)
	    thlist.append(thread3)
	  
	    thread1.start()
	    thread2.start()
	    thread3.start()
	    
	    //join使主线程等待子线程完成后再继续进行，先进行join保证获取到客户端的数据，
	    thread1.join()
	    thread2.join()
	    thread3.join()
	    
	    data1 = thread1.get_result()
	    data2 = thread2.get_result()
	    data3 = thread3.get_result()
	    
	    data = data1 + " " + data2 + " " + data3 + " "
	    print(data)
	c1.close()
	c2.close()
	c3.close()