ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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 ?
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
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.empty():
dataRead= q.get()
message = vipos()
message.Timestamp_translation = float(dataRead[0])
message.translation_x = float(dataRead[1])
message.translation_y = float(dataRead[2])
message.translation_z = float(dataRead[3])
message.Timestamp_rotation = float(dataRead[4])
message.rotation_x = float(dataRead[5])
message.rotation_y = float(dataRead[6])
message.rotation_z = float(dataRead[7])
pub.publish(message)
r.sleep()
decoderProcess.stop()
It may be a long file to follow but it might give you a template to work on. Hope it helps !
For More Details Check : KBS Training