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
