def connect_piksi(self):
self.piksi_driver = TCPDriver(self.piksi_host, self.piksi_port)
#self.piksi_driver = PySerialDriver(self.piksi_port, baud=1000000)
self.piksi_framer = Framer(self.piksi_driver.read,
self.piksi_driver.write,
verbose=False)
self.piksi = Handler(self.piksi_framer)
# Only setup log and settings messages
# We will wait to set up rest until piksi is configured
self.piksi.add_callback(self.callback_sbp_heartbeat,
msg_type=SBP_MSG_HEARTBEAT)
self.piksi.add_callback(self.callback_sbp_log, msg_type=SBP_MSG_LOG)
self.piksi.add_callback(self.callback_sbp_settings_read_resp,
msg_type=SBP_MSG_SETTINGS_READ_RESP)
self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp,
msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_REQ)
self.piksi.add_callback(self.callback_sbp_settings_read_by_index_resp,
msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
self.piksi.add_callback(self.callback_sbp_settings_read_by_index_done,
msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_DONE)
self.piksi.add_callback(self.callback_sbp_startup, SBP_MSG_STARTUP)
self.piksi.start()
def main():
parser = argparse.ArgumentParser(
description="Export position suitable for KML file.")
parser.add_argument("-a",
"--host",
default='192.168.5.51',
help="specify the host address.")
parser.add_argument("-p",
"--port",
default=55555,
help="specify the port to use.")
args = parser.parse_args()
prev = None
# Open a connection to Piksi using TCP
with TCPDriver(args.host, args.port) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
for msg, metadata in source.filter(SBP_MSG_POS_LLH):
cur = (msg.lat, msg.lon)
if prev:
dist = vincenty(prev, cur).meters
#print(dist)
if dist > 0.1:
print("%.16f,%.16f,%.16f" %
(msg.lon, msg.lat, msg.height))
prev = cur
else:
prev = cur
except KeyboardInterrupt:
pass
def test_tcp_logger():
handler = tcp_handler(MsgPrintDep(text=b'abc').to_binary())
ip, port = tcp_server(handler)
t0 = time.time()
sleep = 0.1
timeout = 5.0
cb_context = {'assert_logger_called': False}
def assert_logger(s, **metadata):
cb_context['assert_logger_called'] = True
assert s.preamble == 0x55
assert s.msg_type == 0x10
assert s.sender == 66
assert s.length == 3
assert s.payload == b'abc'
assert s.crc == 0xDAEE
with TCPDriver(ip, port) as driver:
with Handler(Framer(driver.read, None, verbose=False),
autostart=False) as link:
link.add_callback(assert_logger)
link.start()
while True:
if time.time() - t0 > timeout or cb_context[
'assert_logger_called']:
break
time.sleep(sleep)
assert cb_context[
'assert_logger_called'], "SBP msg callback function was not called"
def main():
parser = argparse.ArgumentParser(
description="Swift Navigation SBP Example.")
parser.add_argument("-a",
"--host",
default='localhost',
help="specify the host address.")
parser.add_argument("-p",
"--port",
default=55555,
help="specify the port to use.")
args = parser.parse_args()
# Open a connection to Piksi using TCP
with TCPDriver(args.host, args.port) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
for msg, metadata in source.filter(SBP_MSG_POS_LLH):
# Print out the N, E, D coordinates of the baseline
print(
"%d,%.16f,%.16f,%.16f,%d,%d,%d,%d" %
(msg.tow, msg.lat, msg.lon, msg.height, msg.h_accuracy,
msg.v_accuracy, msg.n_sats, msg.flags))
except KeyboardInterrupt:
pass
def main():
parser = argparse.ArgumentParser(
description="Swift Navigation SBP Example.")
parser.add_argument(
"-a",
"--host",
default='localhost',
help="specify the host address.")
parser.add_argument(
"-p",
"--port",
default=55555,
help="specify the port to use.")
args = parser.parse_args()
# Open a connection to Piksi using TCP
with TCPDriver(args.host, args.port) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
# Print out the N, E, D coordinates of the baseline
print("%.4f,%.4f,%.4f" % (msg.n * 1e-3, msg.e * 1e-3,
msg.d * 1e-3))
except KeyboardInterrupt:
pass
def main():
# Initialize ROS node. We imitate node name present in
# http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
rospy.init_node('piksi')
# Initialize publisher. We imitate publisher names present in
# http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
pub = rospy.Publisher('~navsatfix_best_fix', NavSatFix, queue_size=10)
# Retrive parameters from the ROS parameter serve
# We imitate parameter names present in
# http://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_rtk_ros
host = rospy.get_param('~tcp_addr', '192.168.0.222')
port = rospy.get_param('~tcp_port', 55555)
# Start listening for GNSS data on a TCP socket with a ROS-param-provided
# host and port. This code is almost copy-pasted from examples of the sbp
# package
with TCPDriver(host, port) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
# Filter on SBP_MSG_POS_LLH-messages (LLH stands for
# Longitude Latitude Height)
for msg, metadata in source.filter(SBP_MSG_POS_LLH):
_publish_navsatfix(pub, msg)
if rospy.is_shutdown():
break
except KeyboardInterrupt:
pass
def main():
"""
Get configuration, get driver, and build handler and start it.
"""
args = get_args()
port = args.port[0]
baud = args.baud[0]
if args.tcp:
try:
host, port = port.split(':')
selected_driver = TCPDriver(host, int(port))
except Exception as e:
raise ValueError("Invalid host and/or port: {}".format(e))
else:
selected_driver = serial_link.get_driver(args.ftdi, port, baud)
# Driver with context
with selected_driver as driver:
# Handler with context
with Handler(Framer(driver.read, driver.write)) as link:
print("Creating Settings")
s = Settings(link)
time.sleep(1)
slist = [{
"section": "solution",
"name": "correction_age_max",
"value": "7"
}, {
"section": "solution",
"name": "elevation_mask",
"value": "5"
}]
results = s.write_all(slist, workers=10)
for (res, section, name, value) in results:
if res:
print("write_all failed for {}.{} with error value {}".
format(section, name, res))
print("solution.elevation_mask =",
s.read("solution", "elevation_mask"))
value = input('Enter new solution.elevation_mask value: ')
s.write("solution", "elevation_mask", value)
print("solution.elevation_mask =",
s.read("solution", "elevation_mask"))
l = s.read_all(workers=10)
for setting in l:
print(setting)
print("Release Settings")
s.destroy()
def main():
parser = argparse.ArgumentParser(
description="Swift Navigation SBP Setting Monitor example.")
parser.add_argument("-H",
"--host",
required=True,
help="specify the host address.")
parser.add_argument("-p",
"--port",
default=55556,
help="specify the port to use.")
args = parser.parse_args()
monitor = SettingMonitor()
with TCPDriver(args.host, args.port) as driver:
with Handler(Framer(driver.read, driver.write,
verbose=True)) as link:
driver.flush()
time.sleep(2)
# Capture setting messages
link.add_callback(monitor.capture_setting,
SBP_MSG_SETTINGS_READ_RESP)
link.add_callback(print_setting, SBP_MSG_SETTINGS_READ_RESP)
# Disable spectrum analyzer
link(
MsgSettingsWrite(
setting=b'%s\0%s\0%s\0' %
(b'system_monitor', b'spectrum_analyzer', b'False')))
# Request the value of the system_monitor:spectrum_analyzer setting
link(
MsgSettingsReadReq(
setting=b'%s\0%s\0' %
(b'system_monitor', b'spectrum_analyzer')))
# Wait up to 5 seconds to see the setting we want
specan_off = monitor.wait_for_setting_value(
b'system_monitor', b'spectrum_analyzer', b'False')
assert (specan_off == True)
print("Spectrum analyzer turned off!")
# Request the value of the system_monitor:spectrum_analyzer setting
link(
MsgSettingsReadReq(
setting=b'%s\0%s\0' %
(b'system_monitor', b'spectrum_analyzer')))
# Wait up to 5 seconds to see the setting we (don't) want
specan_off = monitor.wait_for_setting_value(
b'system_monitor', b'spectrum_analyzer', b'True')
assert (specan_off == False)
print("Spectrum analyzer still off!")
def main(args):
"""
Get configuration, get driver, get logger, and build handler and start it.
"""
port = args.port
baud = args.baud
timeout = args.timeout
log_filename = args.logfilename
log_dirname = args.log_dirname
if not log_filename:
log_filename = logfilename()
if log_dirname:
log_filename = os.path.join(log_dirname, log_filename)
append_log_filename = args.append_log_filename
tags = args.tags
# State for handling a networked base stations.
channel = args.channel_id
serial_id = int(args.serial_id) if args.serial_id is not None else None
base = args.base
use_broker = args.broker
# Driver with context
if args.tcp:
try:
host, port = port.split(':')
driver = TCPDriver(host, int(port))
except: # noqa
raise Exception('Invalid host and/or port')
else:
driver = get_driver(args.ftdi,
port,
baud,
args.file,
rtscts=args.rtscts)
# Handler with context
with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
# Logger with context
with get_logger(args.log, log_filename, args.expand_json) as logger:
with get_append_logger(append_log_filename, tags) as append_logger:
link.add_callback(printer, SBP_MSG_PRINT_DEP)
link.add_callback(log_printer, SBP_MSG_LOG)
Forwarder(link, logger).start()
Forwarder(link, append_logger).start()
if use_broker and base and serial_id:
device_id = get_uuid(channel, serial_id)
with HTTPDriver(str(device_id), base) as http:
with Handler(
Framer(http.read, http.write,
args.verbose)) as slink:
Forwarder(slink, swriter(link)).start()
run(args, link)
else:
run(args, link)
def get_tcp_driver(host, port=None):
''' Factory method helper for opening TCPDriver from host and port
'''
try:
if port is None:
host, port = host.split(':')
return TCPDriver(host, int(port), raise_initial_timeout=True)
except ValueError:
raise Exception(
'Invalid format (use ip_address:port): {}'.format(host))
except socket.timeout:
raise Exception(
'TCP connection timed out. Check host: {}'.format(host))
except Exception as e:
raise Exception('Invalid host and/or port: {}'.format(str(e)))
def main():
parser = argparse.ArgumentParser(
description="Swift Navigation SBP FFT monitor example.")
parser.add_argument("-H",
"--host",
required=True,
help="specify the host address.")
parser.add_argument("-p",
"--port",
default=55556,
help="specify the port to use.")
parser.add_argument("-n",
"--num-ffts",
type=int,
default=5,
help="specify the number of FFTs to capture.")
parser.add_argument("-c",
"--channel",
type=int,
default=1,
help="specify the channel to monitor.")
parser.add_argument("-o",
"--output",
type=str,
default="fftmonitor",
help="specify the output filename.")
args = parser.parse_args()
monitor = FFTMonitor()
ch = args.channel
samples = args.num_ffts
with TCPDriver(args.host, args.port) as driver:
with Handler(Framer(driver.read, driver.write, verbose=True)) as link:
driver.flush()
link.add_callback(monitor.capture_fft,
[SBP_MSG_SPECAN, SBP_MSG_SPECAN_DEP])
# Capture 5 FFTs from channel 1
monitor.enable_channel(ch)
while monitor.num_ffts(ch) < samples:
time.sleep(1)
print("Captured %d ffts from channel %d" % (samples, ch))
ffts = monitor.get_ffts(ch)
#monitor.disable_channel(ch)
with open('%s.pickle' % args.output, 'wb') as handle:
pickle.dump(ffts, handle, protocol=pickle.HIGHEST_PROTOCOL)
def get_base_args_driver(args):
driver = None
if args.tcp:
try:
host, port = args.port.split(':')
driver = TCPDriver(host, int(port))
except: # noqa
import traceback
raise Exception('Invalid host and/or port: {0}'.format(
traceback.format_exc()))
else:
driver = get_driver(args.ftdi,
args.port,
args.baud,
args.file,
rtscts=args.rtscts)
return driver
def main():
args = get_args()
port = args.port[0]
baud = args.baud[0]
if args.tcp:
try:
host, port = port.split(':')
selected_driver = TCPDriver(host, int(port))
except:
raise Exception('Invalid host and/or port')
else:
selected_driver = serial_link.get_driver(args.ftdi, port, baud)
# Driver with context
with selected_driver as driver:
# Handler with context
with Handler(Framer(driver.read, driver.write, args.verbose)) as link:
f = FileIO(link)
try:
if args.write:
f.write(args.write[0], open(args.write[0]).read())
elif args.read:
data = f.read(args.read[0])
if args.hex:
print(hexdump(data))
else:
print(data)
elif args.delete:
f.remove(args.delete[0])
elif args.list is not None:
print_dir_listing(f.readdir(args.list[0]))
else:
print("No command given, listing root directory:")
print_dir_listing(f.readdir())
except KeyboardInterrupt:
pass
def main():
# Open a connection to Piksi using TCP
with TCPDriver(BASE_HOST, BASE_PORT) as driver:
with Handler(Framer(driver.read, driver.write, verbose=False)) as source:
base_pos_log(source)
# returns a tuple of the form (latitude, longitude, altitude)
# need to work out how to download email before this runs successfully
# if worst comes to worst, can use local RTKLIB post processing tool
coords = parse_position_data(filename)
# write settings to base station
stngs = settings.Settings(source, DEFAULT_TIMEOUT_SECS)
# TODO: Make sure this works.
# Try contacting SwiftNav about using Settings class to write
# settings. These coordinates are hard-coded for testing. Write
# tuple from parse_position_data to settings in final program.
stngs.write("surveyed_position", "broadcast", "True")
stngs.write("surveyed_position", "surveyed_position.surveyed_lat", "61.2735")
stngs.write("surveyed_position", "surveyed_lon", "-149.85825")
stngs.write("surveyed_position", "surveyed_alt", "15.000")
def navigate(self):
# Select Driver
if self.args.u is False:
driver = TCPDriver('10.0.0.223', '55555')
else:
driver = PySerialDriver('/dev/ttyUSB0',
baud=115200) # 68,72,73,74,65535
# Location of rover, from the top.
# driver.read is the foundliteral output from the tcp driver
# framer turns bytes into SBP messages (swift binary protocol)
# handler is an iterable to read the messages
with Handler(Framer(driver.read, None, verbose=True)) as source:
# reads all the messages in a loop as they are received
for i in range(10):
msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
self.history[i] = msg
# Search for a tennis ball
bt = BallTracker.BallTracker()
print bt
# Start Navigation Loop
while True:
# Get Next Driver Message
msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
for i in range(1, 10):
self.history[i - 1] = self.history[i]
self.history[9] = msg
# Shouldn't We need an elevation?.. that's msg.height
# int of accuracy is [h/v]_accuracy also there is n_sats
self.last_rover_latitude = self.history[0].lat
self.last_rover_longitude = self.history[0].lon
self.rover_latitude = math.radians(msg.lat)
self.rover_longitude = math.radians(msg.lon)
# Get target_longitude, target_latitude,217
# (not yet...height, and number of satellites) - need it in degrees
if self.target_bearing is None and self.distance is None:
self.target_longitude = math.radians(
float(self.target_longitude))
self.target_latitude = math.radians(
float(self.target_latitude))
# If target_bearing and distance were supplied, get target_latitude and target_longitude in degrees
else:
self.distance = float(self.distance)
self.target_bearing = self.get_target_bearing()
self.target_latitude, self.target_longitude = self.get_gps_coordinates(
)
self.target_latitude = math.radians(self.target_latitude)
self.target_longitude = math.radians(self.target_longitude)
self.distance = self.get_distance()
self.leg_distance = self.distance
self.distance_traveled = (self.leg_distance -
self.distance) / self.leg_distance
theta = self.get_target_bearing()
delta_theta = self.get_rover_bearing() - theta
# Get velocity in North, East, Down format
msg2, metadata = source.filter(SBP_MSG_VEL_NED).next()
# Velocity is in mm/s, so convert it to m/s
vel__north = msg2.n * 1000
vel__east = msg2.e * 1000
#print("North: ", vel__north, " East: ", vel__east)
# Calculate rover_heading using the atan2 function
rover_heading = math.atan2(vel__east, vel__north)
# TODO: turn(target_bearing) # something like: if math.abs(target_bearing) > tolerance
# TODO: then turn else drive (distance) drive(distance)
# Need to fix this to output the proper value of theta
#print("Bearing: ", math.degrees(theta), ", Distance: ", self.distance,
# ",Turn:", math.degrees(delta_theta), "My Bearing:", math.degrees(rover_heading))
#print "Ball found: " + str(bt.hasFound()) + ", ball angle: " + str(bt.getAngle()) + ", ball dist: " + str(bt.distance)
if self.distance_traveled > 0.75:
found = bt.hasFound()
if found is True:
print "ball found"
# Give control to the ball tracker
bt.setControl(True)
self.hasControl = False
else:
#Give control to the GPS code
bt.setControl(False)
self.hasControl = True
if self.hasControl is True or True:
# Drive the rover via GPS
self.angle = math.degrees(delta_theta)
#self.speed = 25
# If we're within 2 meters of the destination
if self.get_distance() <= 2:
print(
"Distance is within 2 meters of the destination.")
# A tennis ball wasn't found on the way to the destination.
# Signal that the rover122.41883 has reached the target destination.
self.speed = 0
else:
print("distance is " + str(self.get_distance()) +
" angle is " + str(math.degrees(self.angle)))
self.speed = 25
if bt.hasFound() is True and False:
print "BT has controll"
# Drive the rover via Ball Tracker
self.angle = bt.getAngle()
# If we're within 2 meters of the destination
if bt.distance <= 2:
print(
"Distance is within 2 meters of the destination.")
# A tennis ball wasn't found on the way to the destination.
# Signal that the rover has reached the target destination.
self.speed = 0
else:
self.speed = 25
if show_usage:
usage_str = parser.format_help()
print(usage_str)
usage = ShowUsage(usage_str, error_str)
usage.configure_traits()
sys.exit(1)
selected_driver = None
connection_description = ""
if port and args.tcp:
# Use the TPC driver and interpret port arg as host:port
try:
host, ip_port = port.split(':')
selected_driver = TCPDriver(host, int(ip_port))
connection_description = port
except:
raise Exception('Invalid host and/or port')
sys.exit(1)
elif port and args.file:
# Use file and interpret port arg as the file
print("Using file '%s'" % port)
selected_driver = s.get_driver(args.ftdi, port, baud, args.file)
connection_description = os.path.split(port)[-1]
elif not port:
# Use the gui to get our driver
port_chooser = PortChooser(baudrate=int(args.baud))
is_ok = port_chooser.configure_traits()
ip_address = port_chooser.ip_address
ip_port = port_chooser.ip_port
def main():
args = get_args()
rover_lat = 0.0
rover_long = 0.0
if args.u is False:
driver = TCPDriver('192.168.1.222', '55555')
else:
driver = PySerialDriver('/dev/ttyUSB0', baud=115200) # 68,72,73,74,65535
# Location of rover, from the top.
# driver.read is the literal output from the tcp driver
# framer turns bytes into SBP messages (swift binary protocol)
# handler is an iterable to read the messages
with Handler(Framer(driver.read, None, verbose=True)) as source:
# reads all the messages in a loop as they are received
for i in range(1, 10):
msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
history[i] = msg
while True:
msg, metadata = source.filter(SBP_MSG_POS_LLH).next()
for i in range(1, 10):
history[i-1] = history [i]
history[9] = msg
# Shouldnt We need an elevation?.. that's msg.height
# int of acuracy is [h/v]_acuracy also there is n_sats
last_lat = math.radians(history[0].lat)
last_long = math.radians(history[0].lon)
rover_lat = math.radians(msg.lat)
rover_long = math.radians(msg.lon)
# Get longitude, latitude, (not yet...height, and number of satellites) - need it in degrees
if args.bearing is None and args.distance is None:
longitude = math.radians(float(args.longitude))
latitude = math.radians(float(args.latitude))
# If bearing and distance were supplied, get latitude and longitude in degrees
else:
bearing = float(args.bearing)
distance = float(args.distance)
latitude, longitude = get_gps_coordinate(bearing, distance, rover_lat, rover_long)
latitude = math.radians(latitude)
longitude = math.radians(longitude)
distance = get_distance(longitude, latitude, rover_lat, rover_long)
theta = get_bearing(longitude, latitude, rover_lat, rover_long)
delta_theta = get_my_bearing(rover_lat, rover_long, last_lat, last_long) - theta
# Get velocity in North, East, Down format
msg2, metadata = source.filter(SBP_MSG_VEL_NED).next()
# Velocity is in mm/s, so convert it to m/s
vel__north = msg2.n * 1000
vel__east = msg2.e * 1000
print("North: ", vel__north, " East: ", vel__east)
# Calculate my_heading using the atan2 function
my_heading = math.atan2(vel__east, vel__north)
# TODO: turn(bearing) # something like: if math.abs(bearing) > tolerance then turn else drive (distance)
# TODO: drive(distance)
# Need to fix this to output the proper value of theta
print("Bearing: ", math.degrees(theta), ", Distance: ", distance, ",Turn:", math.degrees(delta_theta),
"My Bearing:", math.degrees(my_heading))
# If we're within 2 meters of the destination
if distance <= 2:
print("Distance is within 2 meters of the destination.")
def __init__(self):
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Initializing")
# Initialize message and publisher structures
self.drive_direction = "forward"
self.comms_enabled = False
self.ncat_process = None
self.previous_x = 0
self.previous_y = 0
# TOPIC: swift_gps/llh/fix_mode
# This topic reports the fix_mode of llh position
# 0 - Invalid
# 1 - Single Point Position (SSP)
# 2 - Differential GNSS (DGNSS)
# 3 - Float RTK
# 4 - Fixed RTK
# 5 - Dead Reckoning
# 6 - Satellite-based Augmentation System (SBAS)
# ROS Publishers
self.pub_imu = rospy.Publisher('/swift_gps/imu/raw',
Imu,
queue_size=10)
self.pub_mag = rospy.Publisher('/swift_gps/mag/raw',
MagneticField,
queue_size=10)
self.pub_llh = rospy.Publisher('/swift_gps/llh/position',
NavSatFix,
queue_size=3)
self.pub_llh_n_sats = rospy.Publisher('/swift_gps/llh/n_sats',
Int32,
queue_size=3)
self.pub_llh_fix_mode = rospy.Publisher('/swift_gps/llh/fix_mode',
Int32,
queue_size=3)
self.pub_ecef_odom = rospy.Publisher(
'/swift_gps/baseline/ecef/position', Odometry, queue_size=3)
# ROS Subscriber
self.sub_rtk_cmd = rospy.Subscriber("/swift_gps/enable_comms", Bool,
self.enable_comms_cb)
self.sub_cmd_vel = rospy.Subscriber("/cmd_vel/managed", TwistStamped,
self.cmd_vel_cb)
# ROS Parameters
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Loading ROS Parameters")
path_piksi_ip_address = rospy.search_param('piksi_ip_address')
self.piksi_ip_address = rospy.get_param(path_piksi_ip_address,
'1.2.3.10')
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Piksi IP address: %s",
self.piksi_ip_address)
path_piksi_port = rospy.search_param('piksi_port')
self.piksi_port = rospy.get_param(path_piksi_port, '55555')
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Piksi Port: %s", self.piksi_port)
path_base_station_ip_address = rospy.search_param(
'base_station_ip_address')
self.base_station_ip_address = rospy.get_param(
path_base_station_ip_address, '111.111.111.111')
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Base Station IP address: %s",
self.base_station_ip_address)
path_base_station_port = rospy.search_param('base_station_port')
self.base_station_port = rospy.get_param(path_base_station_port,
'55555')
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Base Station Port: %s",
self.base_station_port)
path_computer_ip_address = rospy.search_param('computer_ip_address')
self.computer_ip_address = rospy.get_param(path_computer_ip_address,
'1.2.3.55')
rospy.loginfo("[RR_SWIFTNAV_PIKSI] Computer IP address: %s",
self.computer_ip_address)
# Create SwiftNav Callbacks
with TCPDriver(self.piksi_ip_address, self.piksi_port) as driver:
with Handler(Framer(driver.read, driver.write)) as source:
driver.flush()
time.sleep(2)
source.add_callback(self.publish_baseline_msg,
SBP_MSG_BASELINE_NED)
source.add_callback(self.publish_imu_msg, SBP_MSG_IMU_RAW)
source.add_callback(self.publish_mag_msg, SBP_MSG_MAG_RAW)
source.add_callback(self.publish_llh_msg, SBP_MSG_POS_LLH)
source.start
rospy.spin()
def main():
warnings.simplefilter(action="ignore", category=FutureWarning)
logging.basicConfig()
args = None
parser = get_args()
try:
args = parser.parse_args()
port = args.port
baud = args.baud
show_usage = args.help
error_str = ""
except (ArgumentParserError, argparse.ArgumentError,
argparse.ArgumentTypeError) as e:
print(e)
show_usage = True
error_str = "ERROR: " + str(e)
if args and args.toolkit[0] is not None:
ETSConfig.toolkit = args.toolkit[0]
else:
ETSConfig.toolkit = 'qt4'
# Make sure that SIGINT (i.e. Ctrl-C from command line) actually stops the
# application event loop (otherwise Qt swallows KeyboardInterrupt exceptions)
signal.signal(signal.SIGINT, signal.SIG_DFL)
if show_usage:
usage_str = parser.format_help()
print(usage_str)
usage = ShowUsage(usage_str, error_str)
usage.configure_traits()
sys.exit(1)
selected_driver = None
connection_description = ""
if port and args.tcp:
# Use the TPC driver and interpret port arg as host:port
try:
host, ip_port = port.split(':')
selected_driver = TCPDriver(host, int(ip_port))
connection_description = port
except:
raise Exception('Invalid host and/or port')
sys.exit(1)
elif port and args.file:
# Use file and interpret port arg as the file
print("Using file '%s'" % port)
selected_driver = s.get_driver(args.ftdi, port, baud, args.file)
connection_description = os.path.split(port)[-1]
elif not port:
# Use the gui to get our driver
port_chooser = PortChooser(baudrate=int(args.baud))
is_ok = port_chooser.configure_traits()
ip_address = port_chooser.ip_address
ip_port = port_chooser.ip_port
port = port_chooser.port
baud = port_chooser.baudrate
mode = port_chooser.mode
# todo, update for sfw flow control if ever enabled
rtscts = port_chooser.flow_control == flow_control_options_list[1]
if rtscts:
print("using flow control")
# if the user pressed cancel or didn't select anything
if not (port or (ip_address and ip_port)) or not is_ok:
print("No Interface selected!")
sys.exit(1)
else:
# Use either TCP/IP or serial selected from gui
if mode == cnx_type_list[1]:
print("Using TCP/IP at address %s and port %d" %
(ip_address, ip_port))
selected_driver = TCPDriver(ip_address, int(ip_port))
connection_description = ip_address + ":" + str(ip_port)
else:
print("Using serial device '%s'" % port)
selected_driver = s.get_driver(args.ftdi,
port,
baud,
args.file,
rtscts=rtscts)
connection_description = os.path.split(port)[-1] + " @" + str(
baud)
else:
# Use the port passed and assume serial connection
print("Using serial device '%s'" % port)
selected_driver = s.get_driver(args.ftdi,
port,
baud,
args.file,
rtscts=args.rtscts)
connection_description = os.path.split(port)[-1] + " @" + str(baud)
with selected_driver as driver:
with sbpc.Handler(sbpc.Framer(driver.read, driver.write,
args.verbose)) as link:
if args.reset:
link(MsgReset(flags=0))
log_filter = DEFAULT_LOG_LEVEL_FILTER
if args.initloglevel[0]:
log_filter = args.initloglevel[0]
with SwiftConsole(link,
args.update,
log_filter,
cnx_desc=connection_description,
error=args.error,
json_logging=args.log,
log_dirname=args.log_dirname,
override_filename=args.logfilename,
log_console=args.log_console,
networking=args.networking,
serial_upgrade=args.serial_upgrade) as console:
console.configure_traits()
# Force exit, even if threads haven't joined
try:
os._exit(0)
except:
pass
icon=icon,
width=250,
title='Select serial device',
)
def __init__(self):
try:
self.ports = [p for p, _, _ in s.get_ports()]
except TypeError:
pass
if args.tcp:
try:
host, port = port.split(':')
selected_driver = TCPDriver(host, int(port))
except:
raise Exception('Invalid host and/or port')
else:
if not port:
port_chooser = PortChooser()
is_ok = port_chooser.configure_traits()
port = port_chooser.port
if not port or not is_ok:
print "No serial device selected!"
sys.exit(1)
else:
print "Using serial device '%s'" % port
selected_driver = s.get_driver(args.ftdi, port, baud, args.file)