Абонент ROS на розетку в python

Question

Привет всем, я хотел бы реализовать узел python ros, который подписывается на topi c (в моем случае odom) и повторно передает часть сообщения через сокет. Я уже реализовал такую ​​вещь:

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import socket
import sys
from nav_msgs.msg import Odometry
import struct
from time import sleep
import socket, threading


class ClientThread(threading.Thread):
    def __init__(self,clientAddress,clientsocket):
        threading.Thread.__init__(self)
        self.csocket = clientsocket
        print ("New connection added: ", clientAddress)
        rospy.init_node('serverOdom', anonymous=True)
        rospy.Subscriber('odom',Odometry, callback)
        print ("New subscriber 'severOdom' added: ")

    def run(self):
        rospy.spin()


def callback(msg):
    string=str(msg.pose.pose.position.x)+"\n"+str(msg.pose.pose.position.y)+"\n"
    try:
        ##How i can refeer to the socket??
        self.csocket.send(string.encode())
        print (string)
    except socket.error:
        print ("Error client lost")
        ##How to exit from spin()??


if __name__ == '__main__':
    LOCALHOST = "192.168.2.150"
    PORT = 5005
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server.bind((LOCALHOST, PORT))
    print("Server started")
    print("Waiting for client request..")
    while True:
        server.listen(1)
        clientsock, clientAddress = server.accept()
        newthread = ClientThread(clientAddress, clientsock)
        newthread.start()

теперь у меня две проблемы: первая, что внутри функции обратного вызова я не могу использовать

self.csocket.send(string.encode())

и я не знаю, как отправить строку клиенту.

Во-вторых, если клиент отключается, я не могу выйти из функции обратного вызова

какая-то идея? Я новичок с python и ros, заранее спасибо

EDIT: Я нашел решение. может быть, это не изящно, но это работает.

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import socket
import sys
from nav_msgs.msg import Odometry
import struct
from time import sleep
import socket, threading


class ClientThread(threading.Thread):
    def __init__(self,clientAddress,clientsocket):
        threading.Thread.__init__(self)
        self.csocket = clientsocket
        print ("New connection added: ", clientAddress)
        rospy.init_node('serverOdom', anonymous=True)
        rospy.Subscriber('odom',Odometry, callback)
        print ("New subscriber 'severOdom' added: ")

    def run(self):
        while True:
            msg = rospy.wait_for_message("odom", Odometry, timeout=None)
            string=str(msg.pose.pose.position.x)+"\n"+str(msg.pose.pose.position.y)+"\n"
            try:
                self.csocket.send(string.encode())
            except socket.error:
                print ("Error client lost", clientAddress)
                return

def callback(msg):
    pass


if __name__ == '__main__':
    LOCALHOST = ""
    PORT = 5005
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server.bind((LOCALHOST, PORT))
    print("Server started")
    print("Waiting for client request..")
    while True:
        sleep(2)
        server.listen(5)
        clientsock, clientAddress = server.accept()
        newthread = ClientThread(clientAddress, clientsock)
        newthread.start()

Мне нужно было больше клиентов для подключения, и когда соединение с клиентом было потеряно (например, из-за того, что клиент был закрыт), поток, созданный сервером, завершился. с моим решением для каждого потока, созданного сервером, также создается подписчик, возможно, было бы лучше использовать одного подписчика для всех созданных потоков, но пока я не смог этого сделать.

Answer 1

Если вы определяете обратный вызов как член ClientThread, вы можете получить доступ к себе. Пример

class ClientThread(...):
    def callback(self, msg):
        self.csocket.send(string.encode())

Кроме того, ваш поток управления ненадежен. Вся инициализация узла должна происходить в main; вот почему вы запутались. Пример: python узел .

Кроме того, ваш код не совсем понятен, поскольку кажется, что он является одновременно сервером и клиентом. Ex. вот легкое переписывание, но на самом деле это не имеет смысла и ведет себя неправильно. (За исключением того, что части ros должны быть правильными). Например, похоже, что вы пытаетесь использовать потоки ... для выполнения работы ros, в которой каждый узел уже похож на поток.

# server_odom.py

# Ros imports
import rospy
from std_msgs.msg import String
from nav_msgs.msg import Ododmetry

# Sys imports
import sys
import socket
import struct
import threading
from time import sleep

class ClientThread(threading.Thread):
    def __init__(self,clientAddress,clientsocket):
        threading.Thread.__init__(self)
        self.csocket = clientsocket
        print ("New connection added: ", clientAddress)
        rospy.init_node('serverOdom', anonymous=True)
        rospy.Subscriber('odom',Odometry, self.callback)
        print ("New subscriber 'severOdom' added: ")

    def run(self):
        rospy.spin()

    def callback(self, msg):
        string=str(msg.pose.pose.position.x)+"\n"+str(msg.pose.pose.position.y)+"\n"
        try:
            self.csocket.send(string.encode())
            print (string)
        except socket.error:
            print ("Error client lost")
            return

if __name__ == '__main__':
    LOCALHOST = ""
    PORT = 5005
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server.bind((LOCALHOST, PORT))
    print("Server started")
    print("Waiting for client request..")
    while True:
        sleep(2)
        server.listen(5)
        clientsock, clientAddress = server.accept()
        newthread = ClientThread(clientAddress, clientsock)
        newthread.start()

Если вы просто хотите отформатировать одометрическое сообщение в строковый или последовательный буфер, это значительно меньше кода.

# server_odom_min.py

# Ros imports
import rospy
from std_msgs.msg import String
from nav_msgs.msg import Ododmetry

str_pub = None

def callback(self, msg):
    s = String()
    s.data = str(msg.pose.pose.position.x)+"\n"+str(msg.pose.pose.position.y)+"\n"
    if str_pub:
        str_pub.publish(s)

if __name__ == '__main__':
    rospy.init_node('server_odom_min')
    rospy.Subscriber('odom', Odometry, callback)
    str_pub = rospy.Publisher('odom/string', String, queue_size = 1)
    rospy.spin()

Редактировать (в ответ на редактирование): Я вижу, что вы сейчас делаете, моя проблема; второй пример не относится к этому вопросу. Я отредактировал свой первый пример, чтобы отразить то, что вы имеете в виду.