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 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 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 start_subscriber_spin():
rospy.init_node("GPS_Parser", anonymous=True)
gps_pub = rospy.Publisher("GPS", GPSFix, queue_size=100)
gps_port = serial.Serial("/dev/ttyUSB0", 57600)
while not rospy.is_shutdown():
line = gps_port.readline()
tokens = line.split(",")
if (tokens[0] != "$GPGGA" or int(tokens[6]) is 0):
continue
lat = tokens[2]
lat = int(lat[0:2]) + float(lat[2:]) / 60.0
n = tokens[3]
if n is not "N": lat = -lat
lon = tokens[4]
lon = int(lon[0:3]) + float(lon[3:]) / 60.0
w = tokens[5]
if n is "W": lon = -lon
m = GPSFix(latitude=lat, longitude=lon)
gps_pub.publish(m)
pass
def testInvalidGPSFix(self):
rospy.init_node('test_invalid_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_NO_FIX
gps_msg.header.stamp = msg_stamp
# There are two ways in which initialize_origin.py could fail due to getting
# an invalid fix: if it publishes an origin despite not getting a valid fix
# or if it unsubscribes from the gps & fix topics without ever publishing
# an origin.
# This will test for those conditions by waiting until the node has
# subscribed to the topic, then failing if either ROS shuts down, which
# our subscriber will do if it gets an origin message, or if the number of
# connections drops to zero, which means initialize_origin.py subscribed
# but did not publish a message.
r = rospy.Rate(100.0)
timeout = time.time() + 2 # time out after 2 seconds, which should be plenty
node_attached = False
while not rospy.is_shutdown() and time.time() < timeout:
if not node_attached and gps_pub.get_num_connections() > 0:
node_attached = True
if node_attached and gps_pub.get_num_connections() == 0:
break
gps_pub.publish(gps_msg)
r.sleep()
self.assertFalse(self.got_message,
"initialize_origin should not have published an origin.")
self.assertFalse(node_attached and gps_pub.get_num_connections() == 0,
"initialize_origin unsubscribed without getting a valid fix.")
def __init__(self):
rospy.init_node("GPS")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Starting node...")
self.rate = 1.0
self.heading = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.pub_gps_fix = rospy.Publisher('gps_fix', GPSFix, queue_size=1)
self.pub_gps_initial_fix = rospy.Publisher('gps_initial_fix',
GPSFix,
queue_size=1,
latch=True)
self.sub_gps_navsat = rospy.Subscriber('gps_navsat', NavSatFix,
self.gps_navsat_callback)
self.sub_odom = rospy.Subscriber('odom_wheel', Odometry,
self.odom_callback)
self.gps_msg = GPSFix()
self.initial_heading_degrees = rospy.get_param(
'~initial_heading_degrees')
self.count = 0
def __init__(self):
rospy.init_node("GPSCONV")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Starting GPS convertion...")
self.rate = 1.0
self.heading = 0.0
self.latitude = 0.0
self.altitude = 0.0
self.longitude = 0.0
self.pub_gps_fix = rospy.Publisher('gps_fix', GPSFix, queue_size=1)
self.pub_gps_initial_fix = rospy.Publisher('gps_initial_fix',
GPSFix,
queue_size=1,
latch=True)
self.sub_gps_navsat = rospy.Subscriber('gps_navsat', NavSatFix,
self.gps_navsat_callback)
self.sub_gps_heading = rospy.Subscriber('gps_heading', Int32,
self.gps_heading_callback)
self.gps_msg = GPSFix()
self.count = 0
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 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 __init__(self, device="/dev/ttyUSB0", baudrate=115200):
self.sb = SerialBuffer(device, baudrate)
if not self.sb.is_open():
print "Cannot open port"
exit()
self.reset_srv = rospy.Service('rtk_gps/reset', Trigger, self.reset_callback)
self.pub = rospy.Publisher('/rtk_gps/gps', GPSFix)
self.msg = GPSFix()
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 ground_truth_callback(data):
global viz_grndtruth_pub
degrees_from_north = -math.degrees(data.heading_rad) + 90
rospy.loginfo("Got Ground Truth message: (%f, %f, %f)", data.latitude_deg,
data.longitude_deg, degrees_from_north)
viz_msg_groundtruth = GPSFix(latitude=data.latitude_deg,
longitude=data.longitude_deg,
track=degrees_from_north)
viz_grndtruth_pub.publish(viz_msg_groundtruth)
pass
def setUp(self):
self.start_time = time.time()
rospy.Subscriber('/final_waypoints', Lane, self.recv_lane)
rospy.Subscriber('/vehicle/perfect_gps/enhanced_fix', GPSFix,
self.recv_fix)
self.waypoints = []
self.fix = GPSFix()
self.received_waypoints = False
self.received_fix = False
self.max_error = 0
self.max_speed = 0
def run(self):
counter = 0
rospy.loginfo("Simulation started")
while not rospy.is_shutdown():
self.q_pub.publish(Quaternion(self.theta, self.omega, self.bias_yaw, self.delta_mag))
now = rospy.Time.now()
rpy = Vector3Stamped()
rpy.header.stamp = now
rpy.vector.z = self.theta - self.bias_yaw + self.noise(self.noise_yaw)
self.rpy_pub.publish(rpy)
mag = Vector3Stamped()
mag.header = rpy.header
magnitude = 150 + self.noise(self.noise_magnitude_mag)
theta = self.theta - self.delta_mag + self.noise(self.noise_mag)
mag.vector.x = magnitude * math.cos(theta)
mag.vector.y = magnitude * math.sin(theta)
self.mag_pub.publish(mag)
imu = Imu()
imu.header = mag.header
imu.angular_velocity.z = self.omega + self.noise(self.noise_omega)
self.imu_pub.publish(imu)
if (counter % 20) == 0:
gps = GPSFix()
gps.header = rpy.header
gps.speed = 1.0
gps.track = self.theta + self.noise(self.noise_gps)
gps.track = norm_angle(gps.track)
self.gps_pub.publish(gps)
counter += 1
self.bias_index += self.bias_speed * self.dt
self.bias_yaw = 2 * math.pi * math.cos(self.bias_index)
self.theta += self.omega * self.dt
self.theta = norm_angle(self.theta)
rospy.sleep(self.dt)
def waypoints_publisher():
waypoints_pub = rospy.Publisher('WAYPOINTS_VIZ', GPSFix, queue_size=10)
# read from waypoints file, parse JSON, publish
waypoints = open(
"../RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt",
'r')
line = waypoints.readline()
time.sleep(5)
while line:
data = json.loads(line)
viz_msg_waypoint = GPSFix(latitude=data['latitude'],
longitude=data['longitude'])
waypoints_pub.publish(viz_msg_waypoint)
time.sleep(.01)
line = waypoints.readline()
waypoints.close()
pass
def command_callback(data):
global viz_command_pub
global last_latitude
global last_longitude
global last_heading_deg
#steer_cmd should be degrees
rospy.loginfo("got Steering Command msg: %f degrees",
math.degrees(data.steer_cmd_rad))
viz_msg_steering = GPSFix(latitude=last_latitude,
longitude=last_longitude,
track=last_heading_deg -
math.degrees(data.steer_cmd_rad))
viz_command_pub.publish(viz_msg_steering)
pass
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 pose_callback(data):
global viz_pose_pub
global last_latitude
global last_longitude
global last_heading_deg
#data.heading_rad is in Radians from north clockwise, but GPSFix requires degrees from north
degrees_from_north = math.degrees(data.heading_rad)
last_latitude = data.latitude_deg
last_longitude = data.longitude_deg
last_heading_deg = -degrees_from_north + 90
rospy.loginfo("got Pose msg: %f degrees lat, %f degrees long, %f bearing",
last_latitude, last_longitude, last_heading_deg)
viz_msg_heading = GPSFix(latitude=last_latitude,
longitude=last_longitude,
track=last_heading_deg)
viz_pose_pub.publish(viz_msg_heading)
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 location_listener_node():
rospy.init_node("location_listener_node", anonymous=True)
global position_publisher, marker_publisher
global drone_marker, position
drone_marker = Marker()
initDroneMarker()
position = GPSFix()
position_publisher = rospy.Publisher('drone_position',
GPSFix,
queue_size=10)
marker_publisher = rospy.Publisher('drone_marker', Marker, queue_size=10)
rospy.Subscriber("mavros/global_position/local", Odometry, updateAttitude)
rospy.Subscriber("mavros/global_position/global", NavSatFix,
updateLocation)
while not rospy.is_shutdown():
rospy.spin()
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 reset_vars(self):
self.imu_msg = Imu()
self.imu_msg.orientation_covariance = (-1., ) * 9
self.imu_msg.angular_velocity_covariance = (-1., ) * 9
self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu = False
self.gps_msg = NavSatFix()
self.xgps_msg = GPSFix()
self.pub_gps = False
self.vel_msg = TwistStamped()
self.pub_vel = False
self.mag_msg = Vector3Stamped()
self.pub_mag = False
self.temp_msg = Float32()
self.pub_temp = False
self.press_msg = Float32()
self.pub_press = False
self.anin1_msg = UInt16()
self.pub_anin1 = False
self.anin2_msg = UInt16()
self.pub_anin2 = False
self.pub_diag = False
def waypoints_publisher():
waypoints_pub = rospy.Publisher('WAYPOINTS_VIZ', GPSFix, queue_size=10)
# Get location of waypoint file by getting package path and then moving from there
rospack = rospkg.RosPack()
robobuggy_path = rospack.get_path("robobuggy")
config_file_loc = "/config/waypoints.txt"
waypoint_file = robobuggy_path + config_file_loc
# read from waypoints file, parse JSON, publish
waypoints = open(waypoint_file)
line = waypoints.readline()
time.sleep(5)
while line:
data = json.loads(line)
viz_msg_waypoint = GPSFix(latitude=data['latitude'],
longitude=data['longitude'])
waypoints_pub.publish(viz_msg_waypoint)
time.sleep(.01)
line = waypoints.readline()
waypoints.close()
pass
def run():
global device
#dev_logger = utils.enable_logger("digi.xbee.devices", logging.DEBUG)
dev_logger = utils.disable_logger("digi.xbee.devices")
###################################################################################################
# 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']
device = XBeeDevice(usbPort, 57600)
#device = XBeeDevice("/dev/ttyUSB0", 57600)
device.open()
print("Current timeout: %d seconds" % device.get_sync_ops_timeout())
device.set_sync_ops_timeout(0.1)
###################################################################################################
# Get local XBee ID (should be 0, convention for Coordinator)
###################################################################################################
ID = utils.bytes_to_int(device.get_16bit_addr().address)
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'.")
###################################################################################################
# Initialisation
###################################################################################################
#Variables storing the data received by the subscribers
global remote_devices, pubGps, pubEuler, pubLB, pubLE, lastDataStr, lastData
remote_devices = {}
lastDataStr = {}
lastData = {}
pubGps = []
pubEuler = []
pubLB = []
pubLE = []
xnet = device.get_network()
xnet.add_device_discovered_callback(network_callback)
xnet.add_discovery_process_finished_callback(network_finished)
device.add_data_received_callback(data_received)
rate = rospy.Rate(10)
GPSdata = GPSFix()
eulerAnglesData = Vector3()
lineStartData, lineEndData = Pose2D(), Pose2D()
global chosen, mode, cmd
mode = 0
chosen = 0
cmd = Twist()
pygame.init()
screen = pygame.display.set_mode( (640,480) )
pygame.display.set_caption('Python numbers')
###################################################################################################
# Transmission Loop
###################################################################################################
while not rospy.is_shutdown():
if(xnet.is_discovery_running() is False):
xnet.start_discovery_process()
display(screen)
send_command()
rate.sleep()
if(xnet.is_discovery_running()):
xnet.stop_discovery_process()
device.close()
def __init__(self):
self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1)
self.vel_pub = rospy.Publisher('vel', TwistStamped, queue_size=1)
self.extend_fix_pub = rospy.Publisher('extend_fix',
GPSFix,
queue_size=1)
self.heading_pub = rospy.Publisher('heading',
QuaternionStamped,
queue_size=1)
self.time_ref_pub = rospy.Publisher('time_reference',
TimeReference,
queue_size=1)
self.time_ref_source = rospy.get_param('~time_ref_source', None)
self.use_RMC = rospy.get_param('~useRMC', False)
self.valid_fix = False
# epe = estimated position error
self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000)
self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0)
self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1)
self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02)
self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0)
self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0)
self.using_receiver_epe = False
self.lon_std_dev = float("nan")
self.lat_std_dev = float("nan")
self.alt_std_dev = float("nan")
"""Format for this dictionary is the fix type from a GGA message as the key, with
each entry containing a tuple consisting of a default estimated
position error, a NavSatStatus value, and a NavSatFix covariance value."""
self.gps_qualities = {
# Unknown
-1: [
self.default_epe_quality0, NavSatStatus.STATUS_NO_FIX,
NavSatFix.COVARIANCE_TYPE_UNKNOWN
],
# Invalid
0: [
self.default_epe_quality0, NavSatStatus.STATUS_NO_FIX,
NavSatFix.COVARIANCE_TYPE_UNKNOWN
],
# SPS
1: [
self.default_epe_quality1, NavSatStatus.STATUS_FIX,
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
],
# DGPS
2: [
self.default_epe_quality2, NavSatStatus.STATUS_SBAS_FIX,
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
],
# RTK Fix
4: [
self.default_epe_quality4, NavSatStatus.STATUS_GBAS_FIX,
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
],
# RTK Float
5: [
self.default_epe_quality5, NavSatStatus.STATUS_GBAS_FIX,
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
],
# WAAS
9: [
self.default_epe_quality9, NavSatStatus.STATUS_GBAS_FIX,
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
]
}
self.extend_fix = GPSFix()
self.star_map_gps = []
self.star_map_bd = []
self.star_use_gps = []
self.star_use_bd = []
gpspub = rospy.Publisher('fix', NavSatFix)
egpspub = rospy.Publisher('extfix', GPSFix)
gpsVelPub = rospy.Publisher('vel',TwistStamped)
gpstimePub = rospy.Publisher('time_reference', TimeReference)
#Init GPS port
GPSport = rospy.get_param('~port','/dev/ttyUSB0')
GPSrate = rospy.get_param('~baud',4800)
frame_id = rospy.get_param('~frame_id','gps')
sleep_ms = int(rospy.get_param('~sleep_ms',5))
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
trigger = "$GP" + rospy.get_param('~trigger', "GGA")
GPSLock = False
navData = GPSFix()
try:
GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2)
lastmsg = {}
gpstxt = ""
#Read in GPS
while not rospy.is_shutdown():
#read GPS line
c = GPS.read(1)
if not c:
# Nothing, keep waiting
continue
#anything useful will take at least that long to arrive
rospy.sleep(sleep_ms/1000.)
gpstxt += c + GPS.read(GPS.inWaiting())
if not ("\n" in gpstxt):
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))
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 spin_once(self):
def baroPressureToHeight(value):
c1 = 44330.0
c2 = 9.869232667160128300024673081668e-6
c3 = 0.1901975534856
intermediate = 1 - math.pow(c2 * value, c3)
height = c1 * intermediate
return height
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp_data = data.get('Temperature') # float
orient_data = data.get('Orientation Data')
velocity_data = data.get('Velocity')
position_data = data.get('Latlon')
altitude_data = data.get('Altitude')
acc_data = data.get('Acceleration')
gyr_data = data.get('Angular Velocity')
mag_data = data.get('Magnetic')
pressure_data = data.get('Pressure')
time_data = data.get('Timestamp')
gnss_data = data.get('GNSS')
# debug the data from the sensor
# print(data)
# print("\n")
# create messages and default values
"Temp message"
temp_msg = Temperature()
pub_temp = False
"Imu message supported with Modes 1 & 2"
imuraw_msg = Imu()
pub_imuraw = False
imuins_msg = Imu()
pub_imuins = False
"SensorSample message supported with Mode 2"
ss_msg = sensorSample()
pub_ss = False
"Mag message supported with Modes 1 & 2"
mag_msg = Vector3Stamped()
pub_mag = False
"Baro in meters"
baro_msg = FluidPressure()
height_msg = baroSample()
pub_baro = False
"GNSS message supported only with MTi-G-7xx devices"
"Valid only for modes 1 and 2"
gnssPvt_msg = gnssSample()
gps1_msg = NavSatFix()
gps2_msg = GPSFix()
pub_gnssPvt = False
gnssSatinfo_msg = GPSStatus()
pub_gnssSatinfo = False
# All filter related outputs
"Supported in mode 3"
ori_msg = orientationEstimate()
pub_ori = False
"Supported in mode 3 for MTi-G-7xx devices"
vel_msg = velocityEstimate()
pub_vel = False
"Supported in mode 3 for MTi-G-7xx devices"
pos_msg = positionEstimate()
pub_pos = False
# first getting the sampleTimeFine
# note if we are not using ros time, the we should replace the header
# with the time supplied by the GNSS unit
if time_data and not self.use_rostime:
time = time_data['SampleTimeFine']
h.stamp.secs = 100e-6 * time
h.stamp.nsecs = 1e5 * time - 1e9 * math.floor(h.stamp.secs)
# temp data
if temp_data:
temp_msg.temperature = temp_data['Temp']
pub_temp = True
# acceleration data
if acc_data:
if 'accX' in acc_data: # found acceleration
pub_imuraw = True
imuraw_msg.linear_acceleration.x = acc_data['accX']
imuraw_msg.linear_acceleration.y = acc_data['accY']
imuraw_msg.linear_acceleration.z = acc_data['accZ']
if 'freeAccX' in acc_data: # found free acceleration
pub_imuins = True
imuins_msg.linear_acceleration.x = acc_data['freeAccX']
imuins_msg.linear_acceleration.y = acc_data['freeAccY']
imuins_msg.linear_acceleration.z = acc_data['freeAccZ']
if 'Delta v.x' in acc_data: # found delta-v's
pub_ss = True
ss_msg.internal.imu.dv.x = acc_data['Delta v.x']
ss_msg.internal.imu.dv.y = acc_data['Delta v.y']
ss_msg.internal.imu.dv.z = acc_data['Delta v.z']
#else:
# raise MTException("Unsupported message in XDI_AccelerationGroup.")
# gyro data
if gyr_data:
if 'gyrX' in gyr_data: # found rate of turn
pub_imuraw = True
imuraw_msg.angular_velocity.x = gyr_data['gyrX']
imuraw_msg.angular_velocity.y = gyr_data['gyrY']
imuraw_msg.angular_velocity.z = gyr_data['gyrZ']
# note we do not force publishing the INS if we do not use the free acceleration
imuins_msg.angular_velocity.x = gyr_data['gyrX']
imuins_msg.angular_velocity.y = gyr_data['gyrY']
imuins_msg.angular_velocity.z = gyr_data['gyrZ']
if 'Delta q0' in gyr_data: # found delta-q's
pub_ss = True
ss_msg.internal.imu.dq.w = gyr_data['Delta q0']
ss_msg.internal.imu.dq.x = gyr_data['Delta q1']
ss_msg.internal.imu.dq.y = gyr_data['Delta q2']
ss_msg.internal.imu.dq.z = gyr_data['Delta q3']
#else:
# raise MTException("Unsupported message in XDI_AngularVelocityGroup.")
# magfield
if mag_data:
ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX']
ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY']
ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ']
pub_mag = True
if pressure_data:
pub_baro = True
baro_msg.fluid_pressure = pressure_data['Pressure']
height = baroPressureToHeight(pressure_data['Pressure'])
height_msg.height = ss_msg.internal.baro.height = height
# gps fix message
if gnss_data and 'lat' in gnss_data:
pub_gnssPvt = True
# A "3" means that the MTi-G is using the GPS data.
# A "1" means that the MTi-G was using GPS data and is now coasting/dead-reckoning the
# position based on the inertial sensors (the MTi-G is not using GPS data in this mode).
# This is done for 45 seconds, before the MTi-G Mode drops to "0".
# A "0" means that the MTi-G doesn't use GPS data and also that it
# doesn't output position based on the inertial sensors.
if gnss_data['fix'] < 2:
gnssSatinfo_msg.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix
gps1_msg.status.service = 0
else:
gnssSatinfo_msg.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented
gps1_msg.status.service = NavSatStatus.SERVICE_GPS
# lat lon alt
gps1_msg.latitude = gnss_data['lat']
gps1_msg.longitude = gnss_data['lon']
gps1_msg.altitude = gnss_data['hEll']
# covariances
gps1_msg.position_covariance[0] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[4] = math.pow(gnss_data['horzAcc'], 2)
gps1_msg.position_covariance[8] = math.pow(gnss_data['vertAcc'], 2)
gps1_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN
# custom message
gnssPvt_msg.itow = gnss_data['iTOW']
gnssPvt_msg.fix = gnss_data['fix']
gnssPvt_msg.latitude = gnss_data['lat']
gnssPvt_msg.longitude = gnss_data['lon']
gnssPvt_msg.hEll = gnss_data['hEll']
gnssPvt_msg.hMsl = gnss_data['hMsl']
gnssPvt_msg.vel.x = gnss_data['velE']
gnssPvt_msg.vel.y = gnss_data['velN']
gnssPvt_msg.vel.z = gnss_data['velD']
gnssPvt_msg.hAcc = gnss_data['horzAcc']
gnssPvt_msg.vAcc = gnss_data['vertAcc']
gnssPvt_msg.sAcc = gnss_data['speedAcc']
gnssPvt_msg.pDop = gnss_data['PDOP']
gnssPvt_msg.hDop = gnss_data['HDOP']
gnssPvt_msg.vDop = gnss_data['VDOP']
gnssPvt_msg.numSat = gnss_data['nSat']
gnssPvt_msg.heading = gnss_data['heading']
gnssPvt_msg.headingAcc = gnss_data['headingAcc']
if orient_data:
if 'Q0' in orient_data:
pub_imuraw = True
imuraw_msg.orientation.w = orient_data['Q0']
imuraw_msg.orientation.x = orient_data['Q1']
imuraw_msg.orientation.y = orient_data['Q2']
imuraw_msg.orientation.z = orient_data['Q3']
pub_imuins = True
imuins_msg.orientation.w = orient_data['Q0']
imuins_msg.orientation.x = orient_data['Q1']
imuins_msg.orientation.y = orient_data['Q2']
imuins_msg.orientation.z = orient_data['Q3']
elif 'Roll' in orient_data:
pub_ori = True
ori_msg.roll = orient_data['Roll']
ori_msg.pitch = orient_data['Pitch']
ori_msg.yaw = orient_data['Yaw']
else:
raise MTException(
'Unsupported message in XDI_OrientationGroup')
if velocity_data:
pub_vel = True
vel_msg.velE = velocity_data['velX']
vel_msg.velN = velocity_data['velY']
vel_msg.velU = velocity_data['velZ']
if position_data:
pub_pos = True
pos_msg.latitude = position_data['lat']
pos_msg.longitude = position_data['lon']
if altitude_data:
pub_pos = True
tempData = altitude_data['ellipsoid']
pos_msg.hEll = tempData[0]
# publish available information
if pub_imuraw:
imuraw_msg.header = h
self.imuraw_pub.publish(imuraw_msg)
if pub_imuins:
imuins_msg.header = h
self.imuins_pub.publish(imuins_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
temp_msg.header = h
self.temp_pub.publish(temp_msg)
if pub_ss:
ss_msg.header = h
self.ss_pub.publish(ss_msg)
if pub_baro:
baro_msg.header = h
height_msg.header = h
self.baro_pub.publish(baro_msg)
self.height_pub.publish(height_msg)
if pub_gnssPvt:
gnssPvt_msg.header = h
gps1_msg.header = h
self.gnssPvt_pub.publish(gnssPvt_msg)
self.gps1_pub.publish(gps1_msg)
#if pub_gnssSatinfo:
# gnssSatinfo_msg.header = h
# self.gnssSatinfo_pub.publish(gnssSatinfo_msg)
if pub_ori:
ori_msg.header = h
self.ori_pub.publish(ori_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_pos:
pos_msg.header = h
self.pos_pub.publish(pos_msg)
# t -> topic to which read data from (sim is from simulation, gps for real data) #default sim gpsTopic = "sim" #limits for lineal velocity & turn maxVel = 2.0 #2.0 m/s minVel = 0.3 #1.8 m/s maxTurn = 0.8 #0.6 rad/s Kv = 0.2 #Constant for linear velocity Kt = 2 #Constant for turn velocity gpsError = 5 #Max error possible for GPS #GPSFix to cache the GPS data gpsData = GPSFix() #GPSPoint to cache the GPS data gpsDataPoint = GPSPoint() #GPSPoint to cache the GPSTarget target = GPSPoint() #GPSPoint to cache the GPSTarget finalTarget = GPSPoint() #flag to indicate if final goal has been reached goal = False #ball controller status # 0 ball tracker takes control
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi * orient['roll'] / 180.,
pi * orient['pitch'] / 180,
pi * orient['yaw'] / 180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3, :3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Vel')
position_data = data.get('Pos')
rawgps_data = data.get('RAWGPS')
status = data.get('Stat') # int
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., ) * 9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., ) * 9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x & 0x8000:
temp_msg.data = (x - 1 << 16) / 256.
else:
temp_msg.data = x / 256.
if rawgps_data:
if rawgps_data['bGPS'] < self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT'] * 1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data[
'LON'] * 1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT'] * 1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2 * rawgps_data['Hacc'] * 1e-3
xgps_msg.err_vert = 2 * rawgps_data['Vacc'] * 1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
x = imu_data['gyrX']
y = imu_data['gyrY']
z = imu_data['gyrZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.angular_velocity.x = v[0]
imu_msg.angular_velocity.y = v[1]
imu_msg.angular_velocity.z = v[2]
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0.,
0., radians(0.025), 0.,
0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = v[0]
vel_msg.twist.angular.y = v[1]
vel_msg.twist.angular.z = v[2]
pub_vel = True
except KeyError:
pass
try:
x = imu_data['accX']
y = imu_data['accY']
z = imu_data['accZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
imu_msg.linear_acceleration.x = v[0]
imu_msg.linear_acceleration.y = v[1]
imu_msg.linear_acceleration.z = v[2]
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0.,
0.0004)
pub_imu = True
except KeyError:
pass
try:
x = imu_data['magX']
y = imu_data['magY']
z = imu_data['magZ']
v = numpy.array([x, y, z])
v = v.dot(self.R)
mag_msg.vector.x = v[0]
mag_msg.vector.y = v[1]
mag_msg.vector.z = v[2]
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
gpspub = rospy.Publisher('fix', NavSatFix)
egpspub = rospy.Publisher('extfix', GPSFix)
gpsVelPub = rospy.Publisher('vel', TwistStamped)
gpstimePub = rospy.Publisher('time_reference', TimeReference)
#Init GPS port
GPSport = rospy.get_param('~port', '/dev/ttyUSB0')
GPSrate = rospy.get_param('~baud', 4800)
frame_id = rospy.get_param('~frame_id', 'gps')
sleep_ms = int(rospy.get_param('~sleep_ms', 5))
if frame_id[0] != "/":
frame_id = addTFPrefix(frame_id)
time_ref_source = rospy.get_param('~time_ref_source', frame_id)
trigger = "$GP" + rospy.get_param('~trigger', "GGA")
GPSLock = False
navData = GPSFix()
try:
GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2)
lastmsg = {}
gpstxt = ""
#Read in GPS
while not rospy.is_shutdown():
#read GPS line
c = GPS.read(1)
if not c:
# Nothing, keep waiting
continue
#anything useful will take at least that long to arrive
rospy.sleep(sleep_ms / 1000.)
gpstxt += c + GPS.read(GPS.inWaiting())
if not ("\n" in gpstxt):
def __init__(self):
self.fix_pub = rospy.Publisher('GPS/fix', GPSFix, queue_size=1)
self.nmea_pub = rospy.Publisher('GPS/NMEA', String, queue_size=1)
self.valid_fix = False
self.use_RMC = rospy.get_param('~useRMC', False)
self.current_fix = GPSFix()
self.current_nmea = String()
# epe = estimated position error
self.default_epe_quality0 = rospy.get_param('~epe_quality0', 1000000)
self.default_epe_quality1 = rospy.get_param('~epe_quality1', 4.0)
self.default_epe_quality2 = rospy.get_param('~epe_quality2', 0.1)
self.default_epe_quality4 = rospy.get_param('~epe_quality4', 0.02)
self.default_epe_quality5 = rospy.get_param('~epe_quality5', 4.0)
self.default_epe_quality9 = rospy.get_param('~epe_quality9', 3.0)
self.using_receiver_epe = False
self.lon_std_dev = float("nan")
self.lat_std_dev = float("nan")
self.alt_std_dev = float("nan")
"""Format for this dictionary is the fix type from a GGA message as the key, with
each entry containing a tuple consisting of a default estimated
position error, a NavSatStatus value, and a NavSatFix covariance value."""
self.gps_qualities = {
# Unknown
-1: [
self.default_epe_quality0, GPSStatus.STATUS_NO_FIX,
GPSFix.COVARIANCE_TYPE_UNKNOWN
],
# Invalid
0: [
self.default_epe_quality0, GPSStatus.STATUS_NO_FIX,
GPSFix.COVARIANCE_TYPE_UNKNOWN
],
# SPS
1: [
self.default_epe_quality1, GPSStatus.STATUS_FIX,
GPSFix.COVARIANCE_TYPE_APPROXIMATED
],
# DGPS
2: [
self.default_epe_quality2, GPSStatus.STATUS_SBAS_FIX,
GPSFix.COVARIANCE_TYPE_APPROXIMATED
],
# RTK Fix
4: [
self.default_epe_quality4, GPSStatus.STATUS_GBAS_FIX,
GPSFix.COVARIANCE_TYPE_APPROXIMATED
],
# RTK Float
5: [
self.default_epe_quality5, GPSStatus.STATUS_GBAS_FIX,
GPSFix.COVARIANCE_TYPE_APPROXIMATED
],
# WAAS
9: [
self.default_epe_quality9, GPSStatus.STATUS_GBAS_FIX,
GPSFix.COVARIANCE_TYPE_APPROXIMATED
]
}
class Controller:
name = ''
looprate = 10
controller = 0
windMsg = Pose2D()
gpsMsg = GPSFix()
imuMsg = Imu()
velMsg = Twist()
sailAngle = 0.0
rudderAngle = 0.0
def wakeup(self):
self.publishMSG("M")
def loop(self):
control = self.control()
if (control):
self.publishCMD(control)
self.publishOdom()
self.rate.sleep()
def gps(self, data):
self.gpsMsg = data
def imu(self, data):
self.imuMsg = data
def wind(self, data):
self.windMsg = data
def sail(self, data):
self.sailAngle = data.data
def rudder(self, data):
self.rudderAngle = data.data
def vel(self, data):
self.velMsg = data
def publishOdom(self):
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.gpsMsg.latitude
odom_msg.pose.pose.position.y = self.gpsMsg.longitude
odom_msg.pose.pose.position.z = self.gpsMsg.altitude
odom_msg.pose.pose.orientation = self.imuMsg.orientation
odom_msg.twist.twist = self.velMsg
self.odomMsg.publish(odom_msg)
def publishCMD(self, cmd):
self.pubCmd.publish(cmd)
def publishMSG(self, msg):
self.pubMsg.publish(msg)
def __init__(self, name, looprate, mode):
self.name = name
self.looprate = looprate
self.controller = mode
rospy.init_node(self.name, anonymous=True)
self.rate = rospy.Rate(self.looprate)
rospy.Timer(rospy.Duration(30.0), self.wakeup)
self.pubCmd = rospy.Publisher('/sailboat/sailboat_cmd',
Twist,
queue_size=100)
self.pubMsg = rospy.Publisher('/sailboat/sailboat_msg',
String,
queue_size=10)
self.odomMsg = rospy.Publisher('/sailboat/odom',
Odometry,
queue_size=100)
self.gpsSub = rospy.Subscriber('/sailboat/GPS', GPSFix, self.gps)
self.imuSub = rospy.Subscriber('/sailboat/IMU', Imu, self.imu)
self.windSub = rospy.Subscriber('/sailboat/wind', Pose2D, self.wind)
self.sailSub = rospy.Subscriber('/sailboat/sail', Float32, self.sail)
self.rudderSub = rospy.Subscriber('/sailboat/rudder', Float32,
self.rudder)
self.velSub = rospy.Subscriber('/sailboat/IMU_Dv', Twist, self.vel)
time.sleep(1)
self.publishMSG("C" + str(mode))
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Velocity')
position_data = data.get('Position')
rawgps_data = data.get('RAWGPS')
status = data.get('Status') # int
sample = data.get('Sample')
# TODO by @demmeln: use sample counter for timestamp
# correction. Watch out for counter wrap.
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
pub_vel = True
except KeyError:
pass
try:
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
