Ask Your Question

Revision history [back]

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.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