I found this node which i am using lately to communicate with a Windows TCP/IP socket sending data using the NMEA geolocalization Protocol.
it uses a thread to connect in a TCP/IP connection and then a process to decode the message and get it to ros format.
PS : could you post your script to try help you best ?
!/usr/bin/env python
import roslib; roslib.load_manifest('collision_avoidance')
import rospy
from collision_avoidance.msg import vipos
from socket import *
import thread
import time
import multiprocessing
from multiprocessing import Process
from multiprocessing import Queue
def checkcrc(msgSplitted):
checksum = msgSplitted[0]
for i in range (1,len(msgSplitted)-3):
checksum = chr(ord(checksum) ^ ord(msgSplitted[i]))
print int(msgSplitted[-2:],16 )
print ord(checksum)
if (ord(checksum) == int(msgSplitted[-2:],16)):
print "true"
return True
else:
print "(!)- Checksum failed"
return False
client handler thread
fullMessageRead=""
def parseNmeaMessages(msgToParse,q):
""" parse the nmea messages expected
"""
print "will parse following message",msgToParse
if msgToParse.find("VIPOS") == -1 :
return
if checkcrc(msgToParse):
msgSplitted = msgToParse.replace("*",",").replace("\t","").replace("$","")
print msgSplitted
msgSplitted = msgSplitted.split(",")
if len(msgSplitted) > 1:
keyWord = msgSplitted[0]
if keyWord =="VIPOS" :
if len(msgSplitted) >= 9 :
print msgSplitted
Timestamp_trans = msgSplitted[1]
trans_x_value = msgSplitted[2]
trans_y_value = msgSplitted[3]
trans_z_value = msgSplitted[4]
Timestamp_rot = msgSplitted[5]
rot_x_value = msgSplitted[6]
rot_y_value = msgSplitted[7]
rot_z_value = msgSplitted[8]
try :
TRAT = float(Timestamp_trans)
TRAx = float(trans_x_value)
TRAy = float(trans_y_value)
TRAz = float(trans_z_value)
ROTT = float(Timestamp_rot)
ROTx = float(rot_x_value)
ROTy = float(rot_y_value)
ROTz = float(rot_z_value)
q.put([TRAT,TRAx,TRAy,TRAz,ROTT,ROTx,ROTy,ROTz])
except Exception,msg:
print "(!)- cannot convert to float"
#do stuff
else:
pass
else:
print "(!)- bad message :",msgToParse
pass
def getNmeaMessage(msgRead,q):
""" get an nmea message expected like $keywor,value,value*crc\r\n"
"""
global fullMessageRead
fullMessageRead += msgRead
startOfFrame =-1
endOfFrame =-1
endOfFrame= fullMessageRead.find("\n")
while endOfFrame != -1:
startOfFrame = fullMessageRead.find("$")
if (startOfFrame!= -1) and (endOfFrame != -1):
if startOfFrame > endOfFrame:
#error in trame read
fullMessageRead = fullMessageRead[startOfFrame:len(fullMessageRead)]
else:
#get string we need
msgToParse = fullMessageRead[startOfFrame:endOfFrame]
try:
fullMessageRead = fullMessageRead[endOfFrame+1:len(fullMessageRead)]
except:
fullMessageRead =""
parseNmeaMessages(msgToParse,q)
endOfFrame= fullMessageRead.find("\n")
def handler(clientsock,addr,q,BUFSIZ):
data= "1"
timestamp = time.time()
while data != "" :
data = clientsock.recv(BUFSIZ)
getNmeaMessage(data,q)
timestamp = time.time()
#clientsock.close()
class DecoderProcess(Process):
def __init__(self,q):
super(DecoderProcess, self).__init__()
self.HOST = ''
self.PORT = 4321
self.BUFSIZ = 1024
self.ADDR = (self.HOST, self.PORT)
self.q =q
def run(self):
self.serversock = socket(AF_INET, SOCK_STREAM)
self.serversock.bind(self.ADDR)
self.serversock.listen(2)
while not rospy.is_shutdown():
print 'waiting for connection...'
clientsock, addr = self.serversock.accept()
print '...connected from:', addr
thread.start_new_thread(handler, (clientsock, addr,self.q,self.BUFSIZ))
if __name__=='__main__':
rospy.init_node('Vicon_Ros')
pub = rospy.Publisher('vicon', vipos)
r = rospy.Rate(20) # 10hz
q=Queue()
decoderProcess = DecoderProcess(q)
decoderProcess.start()
print "waiting from main thread"
while not rospy.is_shutdown():
#data = "$VIPOS,092751,136,6259,630.33,61.32,0.06,31.66,280511*4B"
#parseNmeaMessages(data,q)
if not q ...
(more)
