def callback( packet ) : if rospy.is_shutdown() : sys.exit() message = GPSFix() message.header.stamp = rospy.Time.now() message.header.frame_id = 'base_link' message.latitude = packet[ 'latitude' ] * 180 / math.pi message.longitude = packet[ 'longitude' ] * 180 / math.pi message.altitude = packet[ 'height' ] message.time = packet[ 'time' ] message.pdop = packet[ 'PDOP' ] message.hdop = packet[ 'HDOP' ] message.vdop = packet[ 'VDOP' ] message.tdop = packet[ 'TDOP' ] message.position_covariance = [ packet[ 'sigma-E' ], packet[ 'cov-EN' ] , 0, packet[ 'cov-EN' ], packet[ 'sigma-N' ], 0, 0, 0, packet[ 'sigma-Up'] ] if packet[ 'cov-EN' ] != 0 : message.position_covariance_type = 3 elif packet[ 'sigma-E' ] != 0 : message.position_covariance_type = 2 else : message.position_covariance_type = 0 if debug : pp.pprint( packet ) pp.pprint( message ) message.status.status = (0 if (message.position_covariance_type > 0) else -1) message.status.position_source = 1 message.status.header = message.header publisherGPSFix.publish( message ) if message.status >= 0 : publisherNavSatFix.publish( NavSatFix( message.header , NavSatStatus( message.status.status, 3 ), message.latitude, message.longitude, message.altitude, message.position_covariance, message.position_covariance_type ) )
def publishGps(pub_gps, frame): message = GPSFix() #header now = rospy.get_rostime() message.header.stamp.secs = now.secs message.header.stamp.nsecs = now.nsecs message.header.frame_id = '/filter_send_gps' #status message.status.header.seq = message.header.seq message.status.header.stamp.secs = now.secs message.status.header.stamp.nsecs = now.nsecs message.status.header.frame_id = frame message.status.satellites_used = nbSat message.status.status = status #autres message.latitude = latitude message.longitude = longitude message.altitude = altitude message.track = route message.speed = speed message.time = time message.hdop = hdop pub_gps.publish(message)
def run(): expected_fleet_size = rospy.get_param('fleetSize', 1) receiving_freq = 10. #Set the speed of the transmission loops ################################################################################################### # Look for XBee USB port, to avoid conflicts with other USB devices ################################################################################################### rospy.init_node('fleetCoordinator', anonymous=True) rospy.loginfo("Looking for XBee...") context = pyudev.Context() usbPort = 'No XBee found' for device in context.list_devices(subsystem='tty'): if 'ID_VENDOR' in device and device['ID_VENDOR'] == 'FTDI': usbPort = device['DEVNAME'] ser = serial.Serial(usbPort, baudrate=57600, timeout=2) ################################################################################################### # Get local XBee ID (should be 0, convention for Coordinator) ################################################################################################### # Enter XBee command mode ser.write('+++') rospy.sleep(1.2) ser.read(10) # Get local XBee adress ser.write('ATMY\r') rospy.sleep(0.1) ans = ser.read(10) ID = eval(ans.split('\r')[0]) # Exit XBee "command mode" ser.write('ATCN\r') if ID == 0: rospy.loginfo("\nHello,\nI am Coordinator " + str(ID) + '\n') else: raise Exception( "This XBee device is not the coordinator of the network,\nlook for the XBee device stamped with 'C'." ) ################################################################################################### # Look for the boats connected in the XBee network ################################################################################################### # To check we have all the boats connected connected = [] #list of connected boats IDs rospy.loginfo("Expecting a fleet of " + str(expected_fleet_size) + " sailboats.") while not rospy.is_shutdown() and len(connected) < expected_fleet_size: #Read a connection message from a sailboat c = ser.read(1) line = '' while c != '#' and not rospy.is_shutdown(): c = ser.read(1) while c != '=' and not rospy.is_shutdown(): line += c c = ser.read(1) line += c #Transmission checks, details in the transmission loop part below check, msgReceived = is_valid(line) if check: words = msgReceived.split() IDboat = int(words[-1]) # if the sailboat is not connected yet, we connect it if IDboat not in connected: connected.append(IDboat) rospy.loginfo('|' + msgReceived + '|') fleetSize = len(connected) #link each ID to a minimal line number in the data storing structure linkDict = {connected[i]: i for i in range(fleetSize)} ser.write(str(fleetSize) + '@' + str(connected) + '@Connected' + '\n') rospy.loginfo("Got boats " + str(connected) + ' connected\n') sleep(5) ################################################################################################### # Initialisation ################################################################################################### #Variables storing the data received by the subscribers global targetString, modeString targetString = str({boat: (0, numpy.pi / 2) for boat in connected}) modeString = str({boat: 0 for boat in connected}) emission_freq = receiving_freq / fleetSize #Frequency of emission for the Coordinator rate = rospy.Rate(receiving_freq) ser.timeout = 0.1 compteur = 0 pub_send_connected = rospy.Publisher("xbee_send_connected", String, queue_size=1) for i in range(2): pub_send_connected.publish(String(data=str(connected))) sleep(2) pub_send_gps = [ rospy.Publisher("xbee_send_gps_" + str(i), GPSFix, queue_size=0) for i in connected ] pub_send_euler_angles = [ rospy.Publisher("xbee_send_euler_angles_" + str(i), Vector3, queue_size=0) for i in connected ] pub_send_line_begin = [ rospy.Publisher("xbee_send_line_begin_" + str(i), Pose2D, queue_size=0) for i in connected ] pub_send_line_end = [ rospy.Publisher("xbee_send_line_end_" + str(i), Pose2D, queue_size=0) for i in connected ] GPSdata = GPSFix() eulerAnglesData = Vector3() lineStartData, lineEndData = Pose2D(), Pose2D() ################################################################################################### #Subscribe to the topics that send the data to communicate to the sailboats. #This data comes from the operator's control systems (keyboard control...) ################################################################################################### # Receives the data relative to the target point # (depends on controlMode, common to all boats) rospy.Subscriber('commands', String, targetTransmission) # Receives the string indicator of the control mode rospy.Subscriber('controlMode', String, modeTransmission) ################################################################################################### # Transmission Loop ################################################################################################### #For statistics and synchronisation emission = -1 #Data storing structure received = ['ID@nothing@nothing@nothing@nothing@nothing@nothing' ] * fleetSize while not rospy.is_shutdown(): emission += 1 c = '' line = '' loopTime = time() #################################################################################################### # Receive useful data from the sailboats # Frame received: # "#####msgSize@ID@windForceString@windDirectionString@gpsString@eulerAnglesString@lineBeginString@lineEndString=====\n" #################################################################################################### #If available, read a line from the XBee while c != '#' and (time() - loopTime) < ( 1 / receiving_freq) and not rospy.is_shutdown(): c = ser.read(1) msgTime = time() if c == '#': while c != '=' and (time() - msgTime) < ( 1 / receiving_freq) and not rospy.is_shutdown(): c = ser.read(1) line += c if "Hello" in line: continue # rospy.loginfo('Line:\n|'+line+'|') # Check message syntax and checkSum and clean the message to use only the useful data check, msgReceived = is_valid(line) if check: # rospy.loginfo("Received\n|" +msgReceived+'|\n') compteur += 1 #Organise the incoming data in the storing structure msgData = msgReceived.split('@') IDboat = int(msgData[0]) received[linkDict[IDboat] - 1] = msgReceived GPSframe = msgData[3] tmpEuler = msgData[4].split(',') eulerAnglesData.x = float(tmpEuler[0]) eulerAnglesData.y = float(tmpEuler[1]) eulerAnglesData.z = float(tmpEuler[2]) tmpStartLine = msgData[5].split(',') lineStartData.x = float(tmpStartLine[0]) lineStartData.y = float(tmpStartLine[1]) lineStartData.theta = float(tmpStartLine[2]) tmpEndLine = msgData[6].split(',') lineEndData.x = float(tmpEndLine[0]) lineEndData.y = float(tmpEndLine[1]) lineEndData.theta = float(tmpEndLine[2]) data = GPSframe.split(',') if GPSframe != "nothing" and data[0] == '$GPGGA' and data[2] != '': #header now = rospy.get_rostime() GPSdata.header.stamp.secs = now.secs GPSdata.header.stamp.nsecs = now.nsecs GPSdata.header.frame_id = '/xbee_send_gps' status = 1 gps_time = float(data[1][0:2]) * 3600 + float( data[1][2:4]) * 60 + float(data[1][4:]) latitude = float(data[2][0:(len(data[2]) - 7)]) + float( data[2][(len(data[2]) - 7):]) / 60 if data[3] == 'S': latitude = -latitude longitude = float(data[4][0:(len(data[4]) - 7)]) + float( data[4][(len(data[4]) - 7):]) / 60 if data[5] == 'W': longitude = -longitude frame_type = float(data[6]) nbSat = float(data[7]) if data[8] != '': hdop = float(data[8]) altitude = float(data[9].split(',')[0]) #status GPSdata.status.header.seq = GPSdata.header.seq GPSdata.status.header.stamp.secs = now.secs GPSdata.status.header.stamp.nsecs = now.nsecs GPSdata.status.header.frame_id = 'GPGGA' GPSdata.status.satellites_used = nbSat GPSdata.status.status = status #autres GPSdata.latitude = latitude GPSdata.longitude = longitude GPSdata.altitude = altitude GPSdata.time = gps_time GPSdata.hdop = hdop pub_send_gps[linkDict[IDboat]].publish(GPSdata) if eulerAnglesData.x != -999: pub_send_euler_angles[linkDict[IDboat]].publish( eulerAnglesData) if lineStartData.x != -999: pub_send_line_begin[linkDict[IDboat]].publish(lineStartData) if lineEndData.x != -999: pub_send_line_end[linkDict[IDboat]].publish(lineEndData) if not check: # rospy.loginfo("Could not read\n|"+line+'|') pass if emission % fleetSize == 0: #We are supposed to have the data of every boat at this point. #Logically, only a transmission failure (simultaneous talk, ...) #can prevent that. ########################################################################################################################################## # Send useful data to the sailboats # Frame emitted: # "#####msgSize@ID1@windForceString1@windDirectionString1@gpsString1@eulerAnglesString1@lineBeginString1@lineEndString1@ID2@...@targetString@modeString=====\n" ########################################################################################################################################## #Collect the data from each boat and the operator and gather them in one string #Creating the core message receivedLines = '' for line in received: receivedLines += line + '@' msg = receivedLines + targetString + '@' + modeString #Generating the checkSum message control size = str(len(msg) + 5) for i in range(len(size), 4): size = '0' + size msg = "#####" + size + '@' + msg + "=====\n" #Emit the message ser.write(msg) # rospy.loginfo("Emitted\n|" + msg + '|') received = ['ID@nothing@nothing@nothing@nothing@nothing@nothing' ] * fleetSize # rospy.loginfo("Emission " + str(emission//fleetSize)) # rate.sleep() if fleetSize == 1: rospy.sleep(0.1 / emission_freq) else: rospy.sleep(0.4 / emission_freq) ################################################################### # Deconnection signal ################################################################### rospy.loginfo("End mission, disconnection of all connected boats\n") ser.write('#####**********=====\n') ser.write('#####**********=====\n') rospy.sleep(1.) ser.write('#####**********=====\n') ser.write('#####**********=====\n') ser.write('#####**********=====\n') rospy.loginfo("Emitted " + str(emission // fleetSize)) rospy.loginfo("Received " + str(compteur - 2))
# will all the information we have # TODO: complete treatment of GPGSV (satellites in view) if '$GPGSA' in lastmsg: fields = lastmsg["$GPGSA"] lockState = int(fields[2]) #print 'lockState=',lockState if lockState == 3: GPSLock = True navData.status.status = GPSStatus.STATUS_FIX else: GPSLock = False navData.status.status = GPSStatus.STATUS_NO_FIX navData.pdop = float(fields[15]) navData.hdop = float(fields[16]) navData.vdop = float(fields[17]) navData.status.satellite_visible_prn = [] for i in range(3, 15): if fields[i] == "": break navData.status.satellite_visible_prn.append( int(fields[i])) navData.status.satellites_visible = len( navData.status.satellite_visible_prn) # In case nobody provides better data: navData.status.satellites_used = navData.status.satellites_visible navData.status.satellite_used_prn = navData.status.satellite_visible_prn if ("$GPRMC" in lastmsg) and GPSLock: fields = lastmsg["$GPRMC"]
# will all the information we have # TODO: complete treatment of GPGSV (satellites in view) if '$GPGSA' in lastmsg: fields = lastmsg["$GPGSA"] lockState = int(fields[2]) #print 'lockState=',lockState if lockState == 3: GPSLock = True navData.status.status = GPSStatus.STATUS_FIX else: GPSLock = False navData.status.status = GPSStatus.STATUS_NO_FIX navData.pdop = float(fields[15]) navData.hdop = float(fields[16]) navData.vdop = float(fields[17]) navData.status.satellite_visible_prn = [] for i in range(3,15): if fields[i]=="": break navData.status.satellite_visible_prn.append(int(fields[i])) navData.status.satellites_visible = len(navData.status.satellite_visible_prn) # In case nobody provides better data: navData.status.satellites_used = navData.status.satellites_visible navData.status.satellite_used_prn = navData.status.satellite_visible_prn if ("$GPRMC" in lastmsg) and GPSLock: fields = lastmsg["$GPRMC"] # Conversion to radian