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 navsatfix_to_gpsfix(navsat_msg):
# Convert sensor_msgs/NavSatFix messages to gps_common/GPSFix messages
gpsfix_msg = GPSFix()
gpsfix_msg.header = navsat_msg.header
gpsfix_msg.status.status = navsat_msg.status.status
gpsfix_msg.status.motion_source = GPSStatus.SOURCE_NONE
gpsfix_msg.status.orientation_source = GPSStatus.SOURCE_NONE
gpsfix_msg.status.position_source = GPSStatus.SOURCE_NONE
if ((navsat_msg.status.service & NavSatStatus.SERVICE_GPS)
or (navsat_msg.status.service & NavSatStatus.SERVICE_GLONASS)
or (navsat_msg.status.service & NavSatStatus.SERVICE_GALILEO)):
gpsfix_msg.status.motion_source = \
gpsfix_msg.status.motion_source | GPSStatus.SOURCE_GPS
gpsfix_msg.status.orientation_source = \
gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_GPS
gpsfix_msg.status.position_source = \
gpsfix_msg.status.position_source | GPSStatus.SOURCE_GPS
if navsat_msg.status.service & NavSatStatus.SERVICE_COMPASS:
gpsfix_msg.status.orientation_source = \
gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_MAGNETIC
gpsfix_msg.latitude = navsat_msg.latitude
gpsfix_msg.longitude = navsat_msg.longitude
gpsfix_msg.altitude = navsat_msg.altitude
gpsfix_msg.position_covariance = navsat_msg.position_covariance
gpsfix_msg.position_covariance_type = navsat_msg.position_covariance_type
return gpsfix_msg
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 testAutoOriginFromGPSFix(self):
rospy.init_node('test_initialize_origin')
gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
gps_msg = GPSFix()
gps_msg.latitude = swri['latitude']
gps_msg.longitude = swri['longitude']
gps_msg.altitude = swri['altitude']
gps_msg.track = swri['heading']
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
gps_pub.publish(gps_msg)
r.sleep()
def testAutoOriginFromGPSFix(self):
rospy.init_node('test_auto_origin_from_gps_fix')
gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2, latch=True)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
gps_msg = GPSFix()
gps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.latitude = swri['latitude']
gps_msg.longitude = swri['longitude']
gps_msg.altitude = swri['altitude']
gps_msg.track = swri['heading']
gps_msg.header.stamp = msg_stamp
gps_pub.publish(gps_msg)
rospy.spin()
self.assertTrue(self.got_origin)
def testAutoOriginFromGPSFix(self):
rospy.init_node('test_initialize_origin')
gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
gps_msg = GPSFix()
gps_msg.latitude = swri['latitude']
gps_msg.longitude = swri['longitude']
gps_msg.altitude = swri['altitude']
gps_msg.track = swri['heading']
gps_msg.header.stamp = msg_stamp
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
gps_pub.publish(gps_msg)
r.sleep()
def testAutoOriginFromGPSFix(self):
rospy.init_node('test_auto_origin_from_gps_fix')
gps_pub = rospy.Publisher('gps', GPSFix, queue_size=2)
origin_sub = self.subscribeToOrigin()
self.test_stamp = True
gps_msg = GPSFix()
gps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.latitude = swri['latitude']
gps_msg.longitude = swri['longitude']
gps_msg.altitude = swri['altitude']
gps_msg.track = swri['heading']
gps_msg.header.stamp = msg_stamp
r = rospy.Rate(100.0)
while not rospy.is_shutdown():
gps_pub.publish(gps_msg)
r.sleep()
def parse_origin(local_xy_origin):
global _gps_fix
local_xy_origins = rospy.get_param('~local_xy_origins', [])
for origin in local_xy_origins:
if origin["name"] == local_xy_origin:
_gps_fix = GPSFix()
_gps_fix.header.frame_id = _local_xy_frame
_gps_fix.status.header.frame_id = local_xy_origin
_gps_fix.latitude = origin["latitude"]
_gps_fix.longitude = origin["longitude"]
_gps_fix.altitude = origin["altitude"]
_gps_fix.track = 90
_origin_pub.publish(_gps_fix)
def parse_origin(local_xy_origin):
global _gps_fix
local_xy_origins = rospy.get_param('~local_xy_origins', [])
for origin in local_xy_origins:
if origin["name"] == local_xy_origin:
_gps_fix = GPSFix()
_gps_fix.header.frame_id = _local_xy_frame
_gps_fix.status.header.frame_id = local_xy_origin
_gps_fix.latitude = origin["latitude"]
_gps_fix.longitude = origin["longitude"]
_gps_fix.altitude = origin["altitude"]
_origin_pub.publish(_gps_fix)
return
def SimGPS():
global pose
#data to be published
gpsData = GPSFix()
pub = rospy.Publisher('gpssim', GPSFix, queue_size=10)
rospy.init_node('SimGPS', anonymous=True)
rospy.Subscriber('base_pose_ground_truth', Odometry, pose_callback)
#load initial point (lat = init_point[0][0], lon = init_point[0][1])
init_point_file = GPSListOfPoints()
init_point_file.loadListFromFile(filename)
#load initial point to an object GPSPoint()
origin = GPSPoint(init_point_file[0])
r = rospy.Rate(1) #1 Hz (rate of /gpssim)
while not rospy.is_shutdown():
#Transform pose to XY point
pose_point = XYPoint(pose.pose.pose.position.x,
pose.pose.pose.position.y)
#Transform pose_point to GPSPoint
gpsData_point = pose_point.XYToGPS(origin)
#Transform GPSPoint to GPSFix data ready to publish in topic
gpsData.latitude = gpsData_point.latitude
gpsData.longitude = gpsData_point.longitude
#Get track from quaternion
quaternion = pyQuaternion()
quaternion.ROSQuaternionTransform(pose.pose.pose.orientation)
#Transform quaternion to jaw rotation (normalized to 360.0)
gpsData.track = XYPoint(0.0, 0.0).angleToBearing(quaternion.getJaw())
#rospy.loginfo(gpsData)
pub.publish(gpsData)
r.sleep()
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))
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(
fields[5][0:3]) + float(fields[5][3:]) / 60
if fields[6] == 'W':
longitude = -longitude
latitude = float(
fields[3][0:2]) + float(fields[3][2:]) / 60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
if "$GPVTG" in lastmsg:
fields = lastmsg["$GPVTG"]
if fields[1] != '':
# Conversion to radian
navData.track = float(fields[1]) * math.pi / 180.
# Conversion from knots to m/s
navData.speed = float(fields[5]) * 0.514444444444
gpsVel.header.stamp = timeNow
gpsVel.twist.linear.x = navData.speed * math.sin(
navData.track)
gpsVel.twist.linear.y = navData.speed * math.cos(
navData.track)
while lat_lon_origin == [[],[]]:
rospy.sleep(0.5)
rospy.loginfo("[{}] Waiting GPS origin".format(node_name))
rospy.loginfo("[{}] Got GPS origin {}".format(node_name,lat_lon_origin))
while not rospy.is_shutdown():
xdot,delta_s=f(x,u)
x = x + dt*xdot
#rospy.loginfo("[{}] x : {}, y:{}".format(node_name,x[0,0],x[1,0]))
#rospy.loginfo("[{}] theta : {}".format(node_name,x[2,0]))
theta_msg.x = x[2,0] # heading
xy_msg.x = x[0,0] # x pos
xy_msg.y = x[1,0] # y pos
xy_msg.z = delta_s
wind_direction_msg.data = psi # wind direction
wind_force_msg.data = awind # wind speed
utm_to_latlon = utm.to_latlon(lat_lon_origin[1][0]-x[1,0],lat_lon_origin[1][1]+x[0,0], lat_lon_origin[1][2],lat_lon_origin[1][3])
gps_msg.latitude = utm_to_latlon[0]
gps_msg.longitude = utm_to_latlon[1]
gps_msg.speed = x[3,0] #speed
pub_send_theta.publish(theta_msg)
pub_send_xy.publish(xy_msg)
pub_send_wind_direction.publish(wind_direction_msg)
pub_send_wind_force.publish(wind_force_msg)
pub_send_gps.publish(gps_msg)
rate.sleep()
publish_time = True
gpstime.header.stamp = gpsVel.header.stamp
gpstime.time_ref = convertNMEATimeToROS(fields[1])
longitude = float(fields[5][0:3]) + float(fields[5][3:])/60
if fields[6] == 'W':
longitude = -longitude
latitude = float(fields[3][0:2]) + float(fields[3][2:])/60
if fields[4] == 'S':
latitude = -latitude
#publish data
navData.latitude = latitude
navData.longitude = longitude
navData.altitude = float('NaN')
navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
if "$GPVTG" in lastmsg:
fields = lastmsg["$GPVTG"]
if fields[1] != '':
# Conversion to radian
navData.track = float(fields[1])*math.pi/180.
# Conversion from knots to m/s
navData.speed = float(fields[5])*0.514444444444
gpsVel.header.stamp = timeNow
gpsVel.twist.linear.x = navData.speed*math.sin(navData.track)
gpsVel.twist.linear.y = navData.speed*math.cos(navData.track)
publish_vel = True
# PointField('rgba', 12, PointField.UINT32, 1),
]
scaled_polygon_pcl = pcl2.create_cloud(header, fields, all_points)
labeled_pointcloud_pub.publish(scaled_polygon_pcl)
bounding_box_pub.publish(marker)
# gps/imu topic gen
with open(gps_imu_file_paths[idx], 'rt') as fp:
line = fp.readline().split(' ')
quat_val = euler_to_quaternion(float(line[3]),float(line[4]),float(line[5]))
navsat = GPSFix()
navsat.header.frame_id = '/map'
navsat.header.stamp = rospy.Time.now()
navsat.latitude = float(line[0])
navsat.longitude = float(line[1])
navsat.altitude = float(line[2])
gps_pub.publish(navsat)
imu = Imu()
imu.header.frame_id = '/map'
imu.header.stamp = rospy.Time.now()
imu.orientation.x = quat_val[0]
imu.orientation.y = quat_val[1]
imu.orientation.z = quat_val[2]
imu.orientation.w = quat_val[3]
imu_pub.publish(imu)
rate.sleep()
