def main():
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as sbp_udp:
sbp_udp.setblocking(1)
sbp_udp.settimeout(None)
sbp_udp.bind((DEFAULT_UDP_BIND_ADDRESS, DEFAULT_UDP_RECV_PORT))
def handle_incoming(msg, **metadata):
sbp_udp.sendto(msg.pack(),
(DEFAULT_UDP_SEND_ADDRESS, DEFAULT_UDP_SEND_PORT))
with PySerialDriver(DEFAULT_SERIAL_PORT,
DEFAULT_SERIAL_BAUD) as driver:
with Handler(Framer(driver.read, driver.write)) as handler:
handler.add_callback(handle_incoming)
print "***"
print "*** Solo Relay Running"
print "***"
print "Sending to %s : %s" % (DEFAULT_UDP_SEND_ADDRESS,
DEFAULT_UDP_SEND_PORT)
print "Recving on %s : %s" % (DEFAULT_UDP_BIND_ADDRESS,
DEFAULT_UDP_RECV_PORT)
try:
while True:
data, addr = sbp_udp.recvfrom(4096)
driver.write(data)
except KeyboardInterrupt:
pass
def reset(port='/dev/ttyUSB0', baud=115200):
'''
Resets the Multi.
Args:
port: serial port [[default='/dev/ttyUSB0']
baud: baud rate [default=115200]
Returns:
None
'''
print('Reading from {} at {}'.format(port, baud))
verify = raw_input('Are you sure want to reset the Multi? Y/[n]: ') or 'n'
if verify == 'Y':
print('Retting the Multi ...')
with PySerialDriver(port, baud) as driver:
reset_sbp = SBP(SBP_MSG_RESET)
reset_sbp.payload = ''
reset_sbp = reset_sbp.pack()
driver.write(reset_sbp)
print('Resetted the Multi.')
else:
print('Skipped resetting.')
def get_driver(use_ftdi=False,
port=SERIAL_PORT,
baud=SERIAL_BAUD,
file=False,
rtscts=False):
"""
Get a driver based on configuration options
Parameters
----------
use_ftdi : bool
For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
port : string
Serial port to read.
baud : int
Serial port baud rate to set.
"""
try:
if use_ftdi:
return PyFTDIDriver(baud)
if file:
return open(port, 'rb')
# HACK - if we are on OSX and the device appears to be a CDC device, open as a binary file
for each in serial.tools.list_ports.comports():
if port == each[0]:
if each[1].startswith("Gadget Serial"):
print "opening a file driver"
return CdcDriver(open(port, 'w+b', 0))
return PySerialDriver(port, baud, rtscts=rtscts)
# if finding the driver fails we should exit with a return code
# currently sbp's py serial driver raises SystemExit, so we trap it
# here
except SystemExit:
sys.exit(1)
def run(self):
'''Thread loop here'''
# Problems? See: https://pylibftdi.readthedocs.org/en/latest/troubleshooting.html
# with PyFTDIDriver(self.sbp_baud) as driver:
try:
with PySerialDriver(self.sbp_port, baud=self.sbp_baud) as driver:
self.driver = driver
self.framer = Framer(driver.read, driver.write)
with Handler(self.framer) as handler:
self.handler = handler
handler.add_callback(self.handle_basestation_obs, msg_type=[SBP_MSG_OBS, SBP_MSG_BASE_POS_LLH])
handler.add_callback(self.handle_surveying, msg_type=[SBP_MSG_POS_LLH])
self.ready()
try:
while not self.wait_on_stop(1.0):
pass
except KeyboardInterrupt:
raise
except socket.error:
raise
except SystemExit:
print "Exit Forced. We're dead."
return
except:
traceback.print_exc()
self.ready()
def main():
'''Retreives data from GNSS module and sends it to a node'''
publisher = rospy.Publisher('swiftnav-gnss', String, queue_size=10)
rospy.init_node('swiftnav')
with PySerialDriver(PORT, baud=1000000) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
while not rospy.is_shutdown():
for msg, _ in source.filter(SBP_MSG_BASELINE_NED):
publisher.publish('{},{},{}'.format(
msg.n * 1e-3, msg.e * 1e-3, msg.d * 1e-3))
def get_imu(): with PySerialDriver(SERIAL_PORT_IN, baud=SERIAL_BAUD_IN) as driver: with Handler(Framer(driver.read, None, verbose=True)) as source: try: for msg, metadata in source.filter(SBP_MSG_IMU_RAW): rate = msg.acc_x/4095.75, msg.acc_y/4095.75, msg.acc_z/4095.75 break except KeyboardInterrupt: pass return rate
def simple_read():
with PySerialDriver(USB_Piksi, baud=1000000) as driver:
with Handler(Framer(driver.read, None, verbose=False)) as source:
for msg, metadata in source.filter(SBP_MSG_POS_LLH):
# Store the latitude, longitude and number of satellites
#print "Lat:", msg.lat, "Lon:", msg.lon, "nSats:", msg.n_sats
SBPComm.lat, SBPComm.lon = msg.lat, msg.lon
SBPComm.height = msg.height
SBPComm.n_sats = msg.n_sats
def get_position(): with PySerialDriver(SERIAL_PORT_IN, baud=SERIAL_BAUD_IN) as driver: with Handler(Framer(driver.read, None, verbose=True)) as source: try: for msg, metadata in source.filter(SBP_MSG_POS_LLH): pos = msg.lat, msg.lon, msg.flags, msg.n_sats break except KeyboardInterrupt: pass return pos
def begin():
SBPComm._driver = PySerialDriver(SBPComm.port, baud=1000000)
SBPComm._handler = Handler(
Framer(SBPComm._driver.read, None, verbose=False))
msg_types = [
SBP_MSG_POS_LLH, SBP_MSG_VEL_NED, SBP_MSG_DOPS,
SBP_MSG_BASELINE_NED, SBP_MSG_BASELINE_HEADING, SBP_MSG_HEARTBEAT
]
SBPComm._handler.add_callback(SBPComm.sbp_cb, msg_types)
SBPComm._handler.start()
def __init__(self):
self.data_lock = threading.Lock()
self.data = None
# Set up the Duro
# Assumes the Duro is plugged into the core via serial
self.d = PySerialDriver('/dev/ttyS0')
self.f = Framer(self.d.read, None, verbose=True)
self.h = Handler(self.f)
self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH_DEP_A)
self.h.add_callback(self._update_gps_callback, SBP_MSG_POS_LLH)
self.h.start()
def main():
args = get_args()
with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
with Handler(driver.read, driver.write) as link:
with ByteLogger(args.filename[0]) as logger:
link.add_callback(logger)
link.start
try:
while True:
time.sleep(0.1)
except KeyboardInterrupt:
pass
def run_application(self):
self.port = Window.port
self.logFile = Window.logFile
self.buad = 115200
# Open a connection to Piksi using the default baud rate (115200) and port (/dev/ttyUSB0)
driver = PySerialDriver(self.port, self.buad)
source = Handler(Framer(driver.read, None, verbose=True))
# Use SBP built in callback function to create callback for posLLH messages
source.add_callback(self.posLLH, SBP_MSG_POS_LLH)
#source.add_callback(baselineCallback, SBP_MSG_BASELINE_NED)
source.start()
def main():
args = get_args()
with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
with Handler(driver.read, driver.write) as handler:
with closing(socket.socket(socket.AF_INET,
socket.SOCK_DGRAM)) as udp:
handler.add_callback(send_udp_callback_generator(udp, args))
handler.start()
try:
while True:
time.sleep(0.1)
except KeyboardInterrupt:
pass
def main():
args = get_args()
address = args.address[0]
udp_port = args.udp_port[0]
with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
with Handler(driver.read, driver.write) as handler:
with UdpLogger(address, udp_port) as udp:
handler.add_callback(udp)
try:
while True:
time.sleep(0.1)
except KeyboardInterrupt:
pass
def get_driver(use_ftdi=False, port=SERIAL_PORT, baud=SERIAL_BAUD):
"""
Get a driver based on configuration options
Parameters
----------
use_ftdi : bool
For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
port : string
Serial port to read.
baud : int
Serial port baud rate to set.
"""
if use_ftdi:
return PyFTDIDriver(baud)
return PySerialDriver(port, baud)
def main():
parser = argparse.ArgumentParser(
description="Swift Navigation SBP Example.")
parser.add_argument("-p",
"--port",
default=['/dev/ttyUSB0'],
nargs=1,
help="specify the serial port to use.")
parser.add_argument("-b",
"--baud",
default=[115200],
nargs=1,
help="specify the baud rate")
parser.add_argument("-f",
"--filename",
default=[DEFAULT_LOG_FILENAME],
nargs=1,
help="specify the name of the log file")
args = parser.parse_args()
print args
# Open a connection to Piksi using the default baud rate (1Mbaud)
# driver = PySerialDriver(args.port[0], args.baud[0])
# source = Handler(Framer(driver.read, None, verbose=True))
# source.add_callback(posLLHCallback, SBP_MSG_POS_LLH)
# source.add_callback(baselineCallback, SBP_MSG_BASE_POS_LLH)
# source.start()
# print SBP_MSG_BASE_POS_LLH
# Open a connection to Piksi using the default baud rate (1Mbaud)
with PySerialDriver(args.port[0], args.baud[0]) 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 msg
except KeyboardInterrupt:
pass
try:
while (1):
time.sleep(.01)
except KeyboardInterrupt:
pass
def main():
parser = argparse.ArgumentParser(description="Swift Navigation SBP Example.")
parser.add_argument("-p", "--port",
default=['/dev/ttyUSB0'], nargs=1,
help="specify the serial port to use.")
args = parser.parse_args()
# Open a connection to Piksi using the default baud rate (1Mbaud)
with PySerialDriver(args.port[0], baud=1000000) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
#for msg, metadata in source.filter(SBP_MSG_BASELINE_NED):
for msg, metadata in source:
# 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)
print msg
except KeyboardInterrupt:
pass
def main():
"""Simple command line interface for running the udp bridge to
forward observation messages
"""
args = get_args()
port = int(args.udp_port[0])
address = args.address[0]
with PySerialDriver(args.serial_port[0], args.baud[0]) as driver:
with Handler(Framer(driver.read, driver.write)) as handler:
with UdpLogger(address, port) as udp:
handler.add_callback(udp, OBS_MSGS)
# Note, we may want to send the ephemeris message in the future
# but the message is too big for MAVProxy right now
try:
while True:
time.sleep(0.1)
except KeyboardInterrupt:
pass
def radio_corrections(q_radio):
global radio_sender # get radio sender id
with PySerialDriver(RADIO_PORT, baud=RADIO_BAUDRATE) as driver:
print(driver.read)
with Handler(Framer(driver.read, None, verbose=False)) as source:
try:
for sbp_msg, metadata in source.filter():
if radio_sender is None:
radio_sender = sbp_msg.sender # get ntrip sender id
if sbp_msg.msg_type == sbp.observation.SBP_MSG_OBS:
q_radio.put(sbp_msg)
else:
if ntrip_sender is not None:
sbp_msg.sender = ntrip_sender
udp.call(sbp_msg)
except KeyboardInterrupt:
pass
def test_tcp_logger():
handler = tcp_handler(MsgPrint(text='abc').to_binary())
ip, port = tcp_server(handler)
port = "socket://%s:%s" % (ip, port)
baud = 115200
t0 = time.time()
sleep = 0.1
def assert_logger(s):
assert s.preamble==0x55
assert s.msg_type==0x10
assert s.sender==66
assert s.length==3
assert s.payload=='abc'
assert s.crc==0xDAEE
with PySerialDriver(port, baud) as driver:
with Handler(driver.read, driver.write, verbose=False) as link:
link.add_callback(assert_logger)
while True:
if (time.time() - t0) < sleep:
break
def main():
global source, exitFlag
rospy.init_node("dGPS_Data", anonymous=True)
parser = argparse.ArgumentParser(
description="Swift Navigation SBP Example.")
parser.add_argument("-p",
"--port",
default=['/dev/ttyUSB0'],
nargs=1,
help="specify the serial port to use.")
args = parser.parse_args()
# Open a connection to Piksi using the default baud rate (1Mbaud)
with PySerialDriver(args.port[0], baud=1000000) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
t1 = threading.Thread(name='baseline_NED', target=baseline_NED)
t2 = threading.Thread(name='Vel_NED', target=velocity_NED)
t3 = threading.Thread(name='dop_Info', target=dop_Info)
t4 = threading.Thread(name='gps_Pos', target=gps_Pos)
dataCollectThread = threading.Thread(name='dataCollect',
target=dataCollect)
t1.start()
t2.start()
t3.start()
t4.start()
dataCollectThread.start()
rospy.spin()
except KeyboardInterrupt:
exitFlag = True
raise SystemExit
def get_driver(use_ftdi=False, port=SERIAL_PORT, baud=SERIAL_BAUD):
"""
Get a driver based on configuration options
Parameters
----------
use_ftdi : bool
For serial driver, use the pyftdi driver, otherwise use the pyserial driver.
port : string
Serial port to read.
baud : int
Serial port baud rate to set.
"""
try:
if use_ftdi:
return PyFTDIDriver(baud)
return PySerialDriver(port, baud)
# if finding the driver fails we should exit with a return code
# currently sbp's py serial driver raises SystemExit, so we trap it
# here
except SystemExit:
sys.exit(1)
def main():
args = get_args()
app = args.app[0]
key = args.key[0]
secret = args.secret[0]
port = args.port[0]
baud = args.baud[0]
rx_channel, rx_event = args.rx_channel_event[0].split(':')
tx_channel, tx_event = args.tx_channel_event[0].split(':')
push = pusher.Pusher(app_id=app,
key=key,
secret=secret,
ssl=True,
port=443)
push_client = pusherclient.Pusher(key, secret=secret)
with PySerialDriver(port, baud) as driver:
with Handler(Framer(driver.read, driver.write)) as handler:
def push_it(sbp_msg):
push.trigger(tx_channel, tx_event, sbp_msg.to_json_dict())
def pull_it(data):
handler(SBP.from_json(data))
def connect_it(data):
push_client.subscribe(rx_channel).bind(rx_event, pull_it)
push_client.connection.bind('pusher:connection_established',
connect_it)
push_client.connect()
try:
for msg, metadata in handler.filter(OBS_MSG_LIST):
push_it(msg)
except KeyboardInterrupt:
pass
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
def __init__(self):
# Print info.
rospy.sleep(0.5) # wait for a while for init to complete before printing
rospy.loginfo(rospy.get_name() + " start")
rospy.loginfo("libsbp version currently used: " + sbp.version.get_git_version())
if Piksi.LIB_SBP_VERSION != sbp.version.get_git_version():
rospy.logwarn("Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!" % (
sbp.version.get_git_version(), Piksi.LIB_SBP_VERSION))
# Open a connection to Piksi.
serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
baud_rate = rospy.get_param('~baud_rate', 1000000)
try:
self.driver = PySerialDriver(serial_port, baud=baud_rate)
except SystemExit:
rospy.logerr("Piksi not found on serial port '%s'", serial_port)
# Create a handler to connect Piksi driver to callbacks.
self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
self.handler = Handler(self.framer)
self.debug_mode = rospy.get_param('~debug_mode', False)
if self.debug_mode:
rospy.loginfo("Piksi driver started in debug mode, every available topic will be published.")
else:
rospy.loginfo("Piksi driver started in normal mode.")
# Corrections over WiFi settings.
self.base_station_mode = rospy.get_param('~base_station_mode', False)
self.udp_broadcast_addr = rospy.get_param('~broadcast_addr', '255.255.255.255')
self.udp_port = rospy.get_param('~broadcast_port', 26078)
self.base_station_ip_for_latency_estimation = rospy.get_param(
'~base_station_ip_for_latency_estimation',
'10.10.50.1')
# Subscribe to OBS messages and relay them via UDP if in base station mode.
if self.base_station_mode:
rospy.loginfo("Starting in base station mode")
self._multicaster = UdpHelpers.SbpUdpMulticaster(self.udp_broadcast_addr, self.udp_port)
self.handler.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
self.handler.add_callback(self.callback_sbp_obs_dep_a, msg_type=SBP_MSG_OBS_DEP_A)
self.handler.add_callback(self.callback_sbp_obs_dep_b, msg_type=SBP_MSG_OBS_DEP_B)
# not sure if SBP_MSG_BASE_POS_LLH or SBP_MSG_BASE_POS_ECEF is better?
self.handler.add_callback(self.callback_sbp_base_pos_llh, msg_type=SBP_MSG_BASE_POS_LLH)
self.handler.add_callback(self.callback_sbp_base_pos_ecef, msg_type=SBP_MSG_BASE_POS_ECEF)
else:
rospy.loginfo("Starting in client station mode")
self._multicast_recv = UdpHelpers.SbpUdpMulticastReceiver(self.udp_port, self.multicast_callback)
# Navsatfix settings.
self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
self.var_rtk_float = rospy.get_param('~var_rtk_float', [25.0, 25.0, 64.0])
self.var_rtk_fix = rospy.get_param('~var_rtk_fix', [0.0049, 0.0049, 0.01])
self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')
# Local ENU frame settings
self.origin_enu_set = False
self.latitude0 = 0.0
self.longitude0 = 0.0
self.altitude0 = 0.0
self.initial_ecef_x = 0.0
self.initial_ecef_y = 0.0
self.initial_ecef_z = 0.0
self.ecef_to_ned_matrix = np.eye(3)
self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
self.transform_child_frame_id = rospy.get_param('~transform_child_frame_id', 'gps_receiver')
if rospy.has_param('~latitude0_deg') and rospy.has_param('~longitude0_deg') and rospy.has_param(
'~altitude0_deg'):
latitude0 = rospy.get_param('~latitude0_deg')
longitude0 = rospy.get_param('~longitude0_deg')
altitude0 = rospy.get_param('~altitude0_deg')
# Set origin ENU frame to coordinate specified by rosparam.
self.init_geodetic_reference(latitude0, longitude0, altitude0)
rospy.loginfo("Origin ENU frame set by rosparam.")
# Advertise topics.
self.publishers = self.advertise_topics()
# Create callbacks.
self.create_callbacks()
# Init messages with "memory".
self.receiver_state_msg = self.init_receiver_state_msg()
self.num_wifi_corrections = self.init_num_corrections_msg()
# corrections over wifi message, if we are not the base station.
if not self.base_station_mode:
# start new thread to periodically ping base station.
threading.Thread(target=self.ping_base_station_over_wifi).start()
self.handler.start()
# Spin.
rospy.spin()
pub_piksidebug.publish(debug_msg)
# Main function.
if __name__ == '__main__':
rospy.init_node('piksi')
#Print info
rospy.sleep(0.5) #wait for a while for init to complete before printing
rospy.loginfo("init_node")
rospy.loginfo("libsbp version currently used: " + sbp.version.get_git_version())
# Open a connection to Piksi using the default baud rate (1Mbaud)
serial_port = rospy.get_param('~serial_port')
try:
driver = PySerialDriver(serial_port, baud=1000000)
except SystemExit:
rospy.logerr("Piksi not found on serial port '%s'", serial_port)
# Create a handler to connect Piksi driver to callbacks
handler = Handler(Framer(driver.read, driver.write, verbose=True))
# Read settings
var_spp_x = rospy.get_param('~var_spp_x')
var_spp_y = rospy.get_param('~var_spp_y')
var_spp_z = rospy.get_param('~var_spp_z')
var_rtk_float_x = rospy.get_param('~var_rtk_float_x')
var_rtk_float_y = rospy.get_param('~var_rtk_float_y')
var_rtk_float_z = rospy.get_param('~var_rtk_float_z')
var_rtk_fix_x = rospy.get_param('~var_rtk_fix_x')
var_rtk_fix_y = rospy.get_param('~var_rtk_fix_y')
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 read_rtk(port='/dev/ttyUSB0', baud=115200):
'''
Reads the RTK output from SwiftNav Piksi, parses the messege and prints.
Piksi's must be configured to give RTK message through the serial port.
NOTE: Current official sbp drivers only support python-2
Args:
port: serial port [[default='/dev/ttyUSB0']
baud: baud rate [default=115200]
Returns:
None
'''
print('Reading from {} at {}'.format(port, baud))
m = RtkMessage()
# t_now = datetime.now().strftime('%Y%m%d%H%M%S')
# out_file = 'GPS_' + t_now + '.txt'
# open a connection to Piksi
# with open(out_file, 'w') as f:
with PySerialDriver(port, baud) as driver:
with Handler(Framer(driver.read, None, verbose=True)) as source:
try:
msg_list = [
SBP_MSG_BASELINE_NED, SBP_MSG_POS_LLH, SBP_MSG_VEL_NED,
SBP_MSG_GPS_TIME
]
for msg, metadata in source.filter(msg_list):
# LLH position in deg-deg-m
if msg.msg_type == 522:
m.lat = msg.lat
m.lon = msg.lon
m.h = msg.height
# RTK position in mm (from base to rover)
elif msg.msg_type == 524:
m.n = msg.n
m.e = msg.e
m.d = msg.d
m.flag = msg.flags
# RTK velocity in mm/s
elif msg.msg_type == 526:
m.v_n = msg.n
m.v_e = msg.e
m.v_d = msg.d
# GPS time
elif msg.msg_type == 258:
m.wn = msg.wn
m.tow = msg.tow # in millis
else:
pass
print(m.whole_string())
# f.write(line)
# f.write('\n')
except KeyboardInterrupt:
pass
return
class PiksiMulti:
LIB_SBP_VERSION_MULTI = '2.2.1' # SBP version used for Piksi Multi.
# Geodetic Constants.
kSemimajorAxis = 6378137
kSemiminorAxis = 6356752.3142
kFirstEccentricitySquared = 6.69437999014 * 0.001
kSecondEccentricitySquared = 6.73949674228 * 0.001
kFlattening = 1 / 298.257223563
def __init__(self):
# Print info.
rospy.sleep(
0.5) # Wait for a while for init to complete before printing.
rospy.loginfo(rospy.get_name() + " start")
rospy.loginfo("libsbp version currently used: " +
sbp.version.get_git_version())
# Check for correct SBP library version dependent on Piksi device.
if PiksiMulti.LIB_SBP_VERSION_MULTI != sbp.version.get_git_version():
rospy.logwarn(
"Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!"
% (sbp.version.get_git_version(),
PiksiMulti.LIB_SBP_VERSION_MULTI))
# Open a connection to Piksi.
serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
baud_rate = rospy.get_param('~baud_rate', 115200)
try:
self.driver = PySerialDriver(serial_port, baud=baud_rate)
except SystemExit:
rospy.logerr("Piksi not found on serial port '%s'", serial_port)
raise
# Create a handler to connect Piksi driver to callbacks.
self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
self.handler = Handler(self.framer)
self.debug_mode = rospy.get_param('~debug_mode', False)
if self.debug_mode:
rospy.loginfo(
"Piksi driver started in debug mode, every available topic will be published."
)
else:
rospy.loginfo("Piksi driver started in normal mode.")
# Corrections over WiFi settings.
self.base_station_mode = rospy.get_param('~base_station_mode', False)
self.udp_broadcast_addr = rospy.get_param('~broadcast_addr',
'255.255.255.255')
self.udp_port = rospy.get_param('~broadcast_port', 26078)
self.base_station_ip_for_latency_estimation = rospy.get_param(
'~base_station_ip_for_latency_estimation', '10.10.10.1')
self.multicaster = []
self.multicast_recv = []
# Navsatfix settings.
self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
self.var_rtk_float = rospy.get_param('~var_rtk_float',
[25.0, 25.0, 64.0])
self.var_rtk_fix = rospy.get_param('~var_rtk_fix',
[0.0049, 0.0049, 0.01])
self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')
# Local ENU frame settings.
self.origin_enu_set = False
self.latitude0 = 0.0
self.longitude0 = 0.0
self.altitude0 = 0.0
self.initial_ecef_x = 0.0
self.initial_ecef_y = 0.0
self.initial_ecef_z = 0.0
self.ecef_to_ned_matrix = np.eye(3)
self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
self.transform_child_frame_id = rospy.get_param(
'~transform_child_frame_id', 'gps_receiver')
if rospy.has_param('~latitude0_deg') and rospy.has_param(
'~longitude0_deg') and rospy.has_param('~altitude0'):
latitude0 = rospy.get_param('~latitude0_deg')
longitude0 = rospy.get_param('~longitude0_deg')
altitude0 = rospy.get_param('~altitude0')
# Set origin ENU frame to coordinate specified by rosparam.
self.init_geodetic_reference(latitude0, longitude0, altitude0)
rospy.loginfo("Origin ENU frame set by rosparam.")
# Advertise topics.
self.publishers = self.advertise_topics()
# Create callbacks.
self.create_callbacks()
# Init messages with "memory".
self.receiver_state_msg = self.init_receiver_state_msg()
self.num_wifi_corrections = self.init_num_corrections_msg()
# Corrections over wifi message, if we are not the base station.
if not self.base_station_mode:
# Start new thread to periodically ping base station.
threading.Thread(target=self.ping_base_station_over_wifi).start()
self.handler.start()
# Reset service.
self.reset_piksi_service = rospy.Service(
rospy.get_name() + '/reset_piksi', std_srvs.srv.SetBool,
self.reset_piksi_service_callback)
# Watchdog timer info
self.watchdog_time = rospy.get_rostime()
self.messages_started = False
# Only have start-up reset in base station mode
if self.base_station_mode:
# Things have 30 seconds to start or we will kill node
rospy.Timer(rospy.Duration(30), self.watchdog_callback, True)
# Spin.
rospy.spin()
def create_callbacks(self):
# Callbacks implemented "manually".
self.handler.add_callback(self.pos_llh_callback,
msg_type=SBP_MSG_POS_LLH)
self.handler.add_callback(self.heartbeat_callback,
msg_type=SBP_MSG_HEARTBEAT)
self.handler.add_callback(self.tracking_state_callback,
msg_type=SBP_MSG_TRACKING_STATE)
self.handler.add_callback(self.uart_state_callback,
msg_type=SBP_MSG_UART_STATE)
# Callbacks generated "automatically".
self.init_callback('baseline_ecef_multi', BaselineEcef,
SBP_MSG_BASELINE_ECEF, MsgBaselineECEF, 'tow', 'x',
'y', 'z', 'accuracy', 'n_sats', 'flags')
self.init_callback('baseline_ned_multi', BaselineNed,
SBP_MSG_BASELINE_NED, MsgBaselineNED, 'tow', 'n',
'e', 'd', 'h_accuracy', 'v_accuracy', 'n_sats',
'flags')
self.init_callback('dops_multi', DopsMulti, SBP_MSG_DOPS, MsgDops,
'tow', 'gdop', 'pdop', 'tdop', 'hdop', 'vdop',
'flags')
self.init_callback('gps_time_multi', GpsTimeMulti, SBP_MSG_GPS_TIME,
MsgGPSTime, 'wn', 'tow', 'ns_residual', 'flags')
self.init_callback('utc_time_multi', UtcTimeMulti, SBP_MSG_UTC_TIME,
MsgUtcTime, 'flags', 'tow', 'year', 'month', 'day',
'hours', 'minutes', 'seconds', 'ns')
self.init_callback('pos_ecef_multi', PosEcef, SBP_MSG_POS_ECEF,
MsgPosECEF, 'tow', 'x', 'y', 'z', 'accuracy',
'n_sats', 'flags')
self.init_callback('vel_ecef', VelEcef, SBP_MSG_VEL_ECEF, MsgVelECEF,
'tow', 'x', 'y', 'z', 'accuracy', 'n_sats', 'flags')
self.init_callback('vel_ned', VelNed, SBP_MSG_VEL_NED, MsgVelNED,
'tow', 'n', 'e', 'd', 'h_accuracy', 'v_accuracy',
'n_sats', 'flags')
self.init_callback('imu_raw', ImuRawMulti, SBP_MSG_IMU_RAW, MsgImuRaw,
'tow', 'tow_f', 'acc_x', 'acc_y', 'acc_z', 'gyr_x',
'gyr_y', 'gyr_z')
self.init_callback('imu_aux', ImuAuxMulti, SBP_MSG_IMU_AUX, MsgImuAux,
'imu_type', 'temp', 'imu_conf')
self.init_callback('log', Log, SBP_MSG_LOG, MsgLog, 'level', 'text')
# do not publish llh message, prefer publishing directly navsatfix_spp or navsatfix_rtk_fix.
# self.init_callback('pos_llh', PosLlh,
# SBP_MSG_POS_LLH, MsgPosLLH,
# 'tow', 'lat', 'lon', 'height', 'h_accuracy', 'v_accuracy', 'n_sats', 'flags')
# Subscribe to OBS messages and relay them via UDP if in base station mode.
if self.base_station_mode:
rospy.loginfo("Starting in base station mode")
self.multicaster = UdpHelpers.SbpUdpMulticaster(
self.udp_broadcast_addr, self.udp_port)
self.handler.add_callback(self.callback_sbp_obs,
msg_type=SBP_MSG_OBS)
# not sure if SBP_MSG_BASE_POS_LLH or SBP_MSG_BASE_POS_ECEF is better?
#self.handler.add_callback(self.callback_sbp_base_pos_llh, msg_type=SBP_MSG_BASE_POS_LLH)
self.handler.add_callback(self.callback_sbp_base_pos_ecef,
msg_type=SBP_MSG_BASE_POS_ECEF)
else:
rospy.loginfo("Starting in client station mode")
self.multicast_recv = UdpHelpers.SbpUdpMulticastReceiver(
self.udp_port, self.multicast_callback)
def init_num_corrections_msg(self):
num_wifi_corrections = InfoWifiCorrections()
num_wifi_corrections.header.seq = 0
num_wifi_corrections.received_corrections = 0
num_wifi_corrections.latency = -1
return num_wifi_corrections
def init_receiver_state_msg(self):
receiver_state_msg = ReceiverState()
receiver_state_msg.num_sat = 0 # Unkown.
receiver_state_msg.rtk_mode_fix = False # Unkown.
receiver_state_msg.sat = [] # Unkown.
receiver_state_msg.cn0 = [] # Unkown.
receiver_state_msg.tracking_running = [] # Unkown.
receiver_state_msg.system_error = 255 # Unkown.
receiver_state_msg.io_error = 255 # Unkown.
receiver_state_msg.swift_nap_error = 255 # Unkown.
receiver_state_msg.external_antenna_present = 255 # Unkown.
return receiver_state_msg
def advertise_topics(self):
"""
Adverties topics.
:return: python dictionary, with topic names used as keys and publishers as values.
"""
publishers = {}
publishers['rtk_fix'] = rospy.Publisher(rospy.get_name() +
'/navsatfix_rtk_fix',
NavSatFix,
queue_size=10)
publishers['spp'] = rospy.Publisher(rospy.get_name() +
'/navsatfix_spp',
NavSatFix,
queue_size=10)
publishers['heartbeat'] = rospy.Publisher(rospy.get_name() +
'/heartbeat',
Heartbeat,
queue_size=10)
publishers['tracking_state'] = rospy.Publisher(rospy.get_name() +
'/tracking_state',
TrackingState,
queue_size=10)
publishers['receiver_state'] = rospy.Publisher(rospy.get_name() +
'/debug/receiver_state',
ReceiverState,
queue_size=10)
publishers['uart_state_multi'] = rospy.Publisher(rospy.get_name() +
'/debug/uart_state',
UartState,
queue_size=10)
# Do not publish llh message, prefer publishing directly navsatfix_spp or navsatfix_rtk_fix.
# publishers['pos_llh'] = rospy.Publisher(rospy.get_name() + '/pos_llh',
# PosLlh, queue_size=10)
publishers['vel_ned'] = rospy.Publisher(rospy.get_name() + '/vel_ned',
VelNed,
queue_size=10)
publishers['log'] = rospy.Publisher(rospy.get_name() + '/log',
Log,
queue_size=10)
# Points in ENU frame.
publishers['enu_pose_fix'] = rospy.Publisher(rospy.get_name() +
'/enu_pose_fix',
PoseWithCovarianceStamped,
queue_size=10)
publishers['enu_point_fix'] = rospy.Publisher(rospy.get_name() +
'/enu_point_fix',
PointStamped,
queue_size=10)
publishers['enu_transform_fix'] = rospy.Publisher(rospy.get_name() +
'/enu_transform_fix',
TransformStamped,
queue_size=10)
publishers['enu_pose_spp'] = rospy.Publisher(rospy.get_name() +
'/enu_pose_spp',
PoseWithCovarianceStamped,
queue_size=10)
publishers['enu_point_spp'] = rospy.Publisher(rospy.get_name() +
'/enu_point_spp',
PointStamped,
queue_size=10)
publishers['enu_transform_spp'] = rospy.Publisher(rospy.get_name() +
'/enu_transform_spp',
TransformStamped,
queue_size=10)
publishers['gps_time_multi'] = rospy.Publisher(rospy.get_name() +
'/gps_time',
GpsTimeMulti,
queue_size=10)
publishers['baseline_ned_multi'] = rospy.Publisher(rospy.get_name() +
'/baseline_ned',
BaselineNed,
queue_size=10)
publishers['utc_time_multi'] = rospy.Publisher(rospy.get_name() +
'/utc_time',
UtcTimeMulti,
queue_size=10)
publishers['imu_raw_multi'] = rospy.Publisher(rospy.get_name() +
'/imu_raw',
ImuRawMulti,
queue_size=10)
publishers['imu_aux_multi'] = rospy.Publisher(rospy.get_name() +
'/debug/imu_aux',
ImuAuxMulti,
queue_size=10)
# Topics published only if in "debug mode".
if self.debug_mode:
publishers['rtk_float'] = rospy.Publisher(rospy.get_name() +
'/navsatfix_rtk_float',
NavSatFix,
queue_size=10)
publishers['vel_ecef'] = rospy.Publisher(rospy.get_name() +
'/vel_ecef',
VelEcef,
queue_size=10)
publishers['enu_pose_float'] = rospy.Publisher(
rospy.get_name() + '/enu_pose_float',
PoseWithCovarianceStamped,
queue_size=10)
publishers['enu_point_float'] = rospy.Publisher(rospy.get_name() +
'/enu_point_float',
PointStamped,
queue_size=10)
publishers['enu_transform_float'] = rospy.Publisher(
rospy.get_name() + '/enu_transform_float',
TransformStamped,
queue_size=10)
publishers['baseline_ecef_multi'] = rospy.Publisher(
rospy.get_name() + '/baseline_ecef',
BaselineEcef,
queue_size=10)
publishers['dops_multi'] = rospy.Publisher(rospy.get_name() +
'/dops',
DopsMulti,
queue_size=10)
publishers['pos_ecef_multi'] = rospy.Publisher(rospy.get_name() +
'/pos_ecef',
PosEcef,
queue_size=10)
if not self.base_station_mode:
publishers['wifi_corrections'] = rospy.Publisher(
rospy.get_name() + '/debug/wifi_corrections',
InfoWifiCorrections,
queue_size=10)
return publishers
def ping_base_station_over_wifi(self):
"""
Ping base station periodically without blocking the driver.
"""
ping_deadline_seconds = 3
interval_between_pings_seconds = 5
while not rospy.is_shutdown():
# Send ping command.
command = [
"ping",
"-w",
str(ping_deadline_seconds), # deadline before stopping attempt
"-c",
"1", # number of pings to send
self.base_station_ip_for_latency_estimation
]
ping = subprocess.Popen(command, stdout=subprocess.PIPE)
out, error = ping.communicate()
# Search for 'min/avg/max/mdev' round trip delay time (rtt) numbers.
matcher = re.compile("(\d+.\d+)/(\d+.\d+)/(\d+.\d+)/(\d+.\d+)")
if matcher.search(out) == None:
# No ping response within ping_deadline_seconds.
# In python write and read operations on built-in type are atomic,
# there's no need to use mutex.
self.num_wifi_corrections.latency = -1
else:
groups_rtt = matcher.search(out).groups()
avg_rtt = groups_rtt[1]
# In python write and read operations on built-in type are atomic,
# there's no need to use mutex.
self.num_wifi_corrections.latency = float(avg_rtt)
time.sleep(interval_between_pings_seconds)
def make_callback(self, sbp_type, ros_message, pub, attrs):
"""
Dynamic generator for callback functions for message types from
the SBP library.
Inputs: 'sbp_type' name of SBP message type.
'ros_message' ROS message type with SBP format.
'pub' ROS publisher for ros_message.
'attrs' array of attributes in SBP/ROS message.
Returns: callback function 'callback'.
"""
def callback(msg, **metadata):
sbp_message = sbp_type(msg)
ros_message.header.stamp = rospy.Time.now()
for attr in attrs:
if attr == 'flags':
# Least significat three bits of flags indicate status.
if (msg.flags & 0x07) == 0:
return # Invalid message, do not publish it.
setattr(ros_message, attr, getattr(sbp_message, attr))
pub.publish(ros_message)
return callback
def init_callback(self, topic_name, ros_datatype, sbp_msg_type,
callback_data_type, *attrs):
"""
Initializes the callback function for an SBP
message type.
Inputs: 'topic_name' name of ROS topic for publisher
'ros_datatype' ROS custom message type
'sbp_msg_type' name of SBP message type for callback function
'callback_data_type' name of SBP message type for SBP library
'*attrs' array of attributes in ROS/SBP message
"""
# Check that required topic has been advertised.
if topic_name in self.publishers:
ros_message = ros_datatype()
# Add callback function.
pub = self.publishers[topic_name]
callback_function = self.make_callback(callback_data_type,
ros_message, pub, attrs)
self.handler.add_callback(callback_function, msg_type=sbp_msg_type)
def callback_sbp_obs(self, msg, **metadata):
# rospy.logwarn("CALLBACK SBP OBS")
self.multicaster.sendSbpPacket(msg)
def callback_sbp_obs_dep_a(self, msg, **metadata):
# rospy.logwarn("CALLBACK SBP OBS DEP A")
self.multicaster.sendSbpPacket(msg)
def callback_sbp_obs_dep_b(self, msg, **metadata):
# rospy.logwarn("CALLBACK SBP OBS DEP B")
self.multicaster.sendSbpPacket(msg)
def callback_sbp_base_pos_llh(self, msg, **metadata):
# rospy.logwarn("CALLBACK SBP OBS BASE LLH")
self.multicaster.sendSbpPacket(msg)
def callback_sbp_base_pos_ecef(self, msg, **metadata):
# rospy.logwarn("CALLBACK SBP OBS BASE LLH")
self.multicaster.sendSbpPacket(msg)
def multicast_callback(self, msg, **metadata):
# rospy.logwarn("MULTICAST Callback")
if self.framer:
self.framer(msg, **metadata)
# Publish debug message about wifi corrections, if enabled.
self.num_wifi_corrections.header.seq += 1
self.num_wifi_corrections.header.stamp = rospy.Time.now()
self.num_wifi_corrections.received_corrections += 1
if not self.base_station_mode:
self.publishers['wifi_corrections'].publish(
self.num_wifi_corrections)
else:
rospy.logwarn(
"Received external SBP msg, but Piksi not connected.")
def watchdog_callback(self, event):
if ((rospy.get_rostime() - self.watchdog_time).to_sec() > 10.0):
rospy.logwarn("Heartbeat failed, watchdog triggered.")
if self.base_station_mode:
rospy.signal_shutdown(
"Watchdog triggered, was gps disconnected?")
def pos_llh_callback(self, msg_raw, **metadata):
msg = MsgPosLLH(msg_raw)
# Invalid messages.
if msg.flags == PosLlhMulti.FIX_MODE_INVALID:
return
# SPP GPS messages.
elif msg.flags == PosLlhMulti.FIX_MODE_SPP:
self.publish_spp(msg.lat, msg.lon, msg.height)
#TODO: Differential GNSS (DGNSS)
#elif msg.flags == PosLlhMulti.FIX_MODE_DGNSS
# RTK GPS messages.
elif msg.flags == PosLlhMulti.FIX_MODE_FLOAT_RTK and self.debug_mode:
self.publish_rtk_float(msg.lat, msg.lon, msg.height)
elif msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK:
# Use first RTK fix to set origin ENU frame, if it was not set by rosparam.
if not self.origin_enu_set:
self.init_geodetic_reference(msg.lat, msg.lon, msg.height)
self.publish_rtk_fix(msg.lat, msg.lon, msg.height)
# Update debug msg and publish.
self.receiver_state_msg.rtk_mode_fix = True if (
msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK) else False
self.publish_receiver_state_msg()
def publish_spp(self, latitude, longitude, height):
self.publish_gps_point(latitude, longitude, height, self.var_spp,
NavSatStatus.STATUS_FIX, self.publishers['spp'],
self.publishers['enu_pose_spp'],
self.publishers['enu_point_spp'],
self.publishers['enu_transform_spp'])
def publish_rtk_float(self, latitude, longitude, height):
self.publish_gps_point(latitude, longitude, height, self.var_rtk_float,
NavSatStatus.STATUS_GBAS_FIX,
self.publishers['rtk_float'],
self.publishers['enu_pose_float'],
self.publishers['enu_point_float'],
self.publishers['enu_transform_float'])
def publish_rtk_fix(self, latitude, longitude, height):
self.publish_gps_point(latitude, longitude, height, self.var_rtk_fix,
NavSatStatus.STATUS_GBAS_FIX,
self.publishers['rtk_fix'],
self.publishers['enu_pose_fix'],
self.publishers['enu_point_fix'],
self.publishers['enu_transform_fix'])
def publish_gps_point(self, latitude, longitude, height, variance, status,
pub_navsatfix, pub_pose, pub_point, pub_transform):
# Navsatfix message.
navsatfix_msg = NavSatFix()
navsatfix_msg.header.stamp = rospy.Time.now()
navsatfix_msg.header.frame_id = self.navsatfix_frame_id
navsatfix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
navsatfix_msg.status.service = NavSatStatus.SERVICE_GPS
navsatfix_msg.latitude = latitude
navsatfix_msg.longitude = longitude
navsatfix_msg.altitude = height
navsatfix_msg.status.status = status
navsatfix_msg.position_covariance = [
variance[0], 0, 0, 0, variance[1], 0, 0, 0, variance[2]
]
# Local Enu coordinate.
(east, north, up) = self.geodetic_to_enu(latitude, longitude, height)
# Pose message.
pose_msg = PoseWithCovarianceStamped()
pose_msg.header.stamp = navsatfix_msg.header.stamp
pose_msg.header.frame_id = self.enu_frame_id
pose_msg.pose = self.enu_to_pose_msg(east, north, up, variance)
# Point message.
point_msg = PointStamped()
point_msg.header.stamp = navsatfix_msg.header.stamp
point_msg.header.frame_id = self.enu_frame_id
point_msg.point = self.enu_to_point_msg(east, north, up)
# Transform message.
transform_msg = TransformStamped()
transform_msg.header.stamp = navsatfix_msg.header.stamp
transform_msg.header.frame_id = self.enu_frame_id
transform_msg.child_frame_id = self.transform_child_frame_id
transform_msg.transform = self.enu_to_transform_msg(east, north, up)
# Publish.
pub_navsatfix.publish(navsatfix_msg)
pub_pose.publish(pose_msg)
pub_point.publish(point_msg)
pub_transform.publish(transform_msg)
def heartbeat_callback(self, msg_raw, **metadata):
msg = MsgHeartbeat(msg_raw)
# Let watchdag know messages are still arriving
self.watchdog_time = rospy.get_rostime()
# Start watchdog with 10 second timeout to ensure we keep getting gps
if (not self.messages_started):
self.messages_started = True
rospy.Timer(rospy.Duration(10), self.watchdog_callback)
heartbeat_msg = Heartbeat()
heartbeat_msg.header.stamp = rospy.Time.now()
heartbeat_msg.system_error = msg.flags & 0x01
heartbeat_msg.io_error = msg.flags & 0x02
heartbeat_msg.swift_nap_error = msg.flags & 0x04
heartbeat_msg.sbp_minor_version = (msg.flags & 0xFF00) >> 8
heartbeat_msg.sbp_major_version = (msg.flags & 0xFF0000) >> 16
heartbeat_msg.external_antenna_present = (msg.flags & 0x80000000) >> 31
self.publishers['heartbeat'].publish(heartbeat_msg)
# Update debug msg and publish.
self.receiver_state_msg.system_error = heartbeat_msg.system_error
self.receiver_state_msg.io_error = heartbeat_msg.io_error
self.receiver_state_msg.swift_nap_error = heartbeat_msg.swift_nap_error
self.receiver_state_msg.external_antenna_present = heartbeat_msg.external_antenna_present
self.publish_receiver_state_msg()
def tracking_state_callback(self, msg_raw, **metadata):
msg = MsgTrackingState(msg_raw)
tracking_state_msg = TrackingState()
tracking_state_msg.header.stamp = rospy.Time.now()
tracking_state_msg.state = []
tracking_state_msg.sat = []
tracking_state_msg.code = []
tracking_state_msg.cn0 = []
for single_tracking_state in msg.states:
# Take only running tracking.
track_running = single_tracking_state.state & 0x01
if track_running:
tracking_state_msg.state.append(single_tracking_state.state)
tracking_state_msg.sat.append(single_tracking_state.sid.sat)
tracking_state_msg.code.append(single_tracking_state.sid.code)
tracking_state_msg.cn0.append(single_tracking_state.cn0)
# Publish if there's at least one element in each array.
if len(tracking_state_msg.state) \
and len(tracking_state_msg.sat) \
and len(tracking_state_msg.code) \
and len(tracking_state_msg.cn0):
self.publishers['tracking_state'].publish(tracking_state_msg)
# Update debug msg and publish.
self.receiver_state_msg.num_sat = 0 # Count number of satellites used to track.
for tracking_running in tracking_state_msg.state:
self.receiver_state_msg.num_sat += tracking_running
self.receiver_state_msg.sat = tracking_state_msg.sat
self.receiver_state_msg.cn0 = tracking_state_msg.cn0
self.receiver_state_msg.tracking_running = tracking_state_msg.state
self.publish_receiver_state_msg()
# def utc_time_callback(self, msg_raw, **metadata):
# msg = MsgUtcTime(msg_raw)
#
# # check i message is valid
# if msg.flags & 0x01 == True: # msg valid TODO: use bitmask instead
# # TODO: calc delta_t to rospy.Time.now()
# # delta_t_vec.append(delta_t)
# # self.delta_t_MA = moving_average_filter(delta_t_vec, N)
# return
# else: # msg invalid
# return
def publish_receiver_state_msg(self):
self.receiver_state_msg.header.stamp = rospy.Time.now()
self.publishers['receiver_state'].publish(self.receiver_state_msg)
def uart_state_callback(self, msg_raw, **metadata):
msg = MsgUartState(msg_raw)
uart_state_msg = UartState()
uart_state_msg.header.stamp = rospy.Time.now()
uart_state_msg.uart_a_tx_throughput = msg.uart_a.tx_throughput
uart_state_msg.uart_a_rx_throughput = msg.uart_a.rx_throughput
uart_state_msg.uart_a_crc_error_count = msg.uart_a.crc_error_count
uart_state_msg.uart_a_io_error_count = msg.uart_a.io_error_count
uart_state_msg.uart_a_tx_buffer_level = msg.uart_a.tx_buffer_level
uart_state_msg.uart_a_rx_buffer_level = msg.uart_a.rx_buffer_level
uart_state_msg.uart_b_tx_throughput = msg.uart_b.tx_throughput
uart_state_msg.uart_b_rx_throughput = msg.uart_b.rx_throughput
uart_state_msg.uart_b_crc_error_count = msg.uart_b.crc_error_count
uart_state_msg.uart_b_io_error_count = msg.uart_b.io_error_count
uart_state_msg.uart_b_tx_buffer_level = msg.uart_b.tx_buffer_level
uart_state_msg.uart_b_rx_buffer_level = msg.uart_b.rx_buffer_level
uart_state_msg.uart_ftdi_tx_throughput = msg.uart_ftdi.tx_throughput
uart_state_msg.uart_ftdi_rx_throughput = msg.uart_ftdi.rx_throughput
uart_state_msg.uart_ftdi_crc_error_count = msg.uart_ftdi.crc_error_count
uart_state_msg.uart_ftdi_io_error_count = msg.uart_ftdi.io_error_count
uart_state_msg.uart_ftdi_tx_buffer_level = msg.uart_ftdi.tx_buffer_level
uart_state_msg.uart_ftdi_rx_buffer_level = msg.uart_ftdi.rx_buffer_level
uart_state_msg.latency_avg = msg.latency.avg
uart_state_msg.latency_lmin = msg.latency.lmin
uart_state_msg.latency_lmax = msg.latency.lmax
uart_state_msg.latency_current = msg.latency.current
uart_state_msg.obs_period_avg = msg.obs_period.avg
uart_state_msg.obs_period_pmin = msg.obs_period.pmin
uart_state_msg.obs_period_pmax = msg.obs_period.pmax
uart_state_msg.obs_period_current = msg.obs_period.current
self.publishers['uart_state_multi'].publish(uart_state_msg)
def init_geodetic_reference(self, latitude, longitude, altitude):
if self.origin_enu_set:
return
self.latitude0 = math.radians(latitude)
self.longitude0 = math.radians(longitude)
self.altitude0 = altitude
(self.initial_ecef_x, self.initial_ecef_y,
self.initial_ecef_z) = self.geodetic_to_ecef(latitude, longitude,
altitude)
# Compute ECEF to NED.
phiP = math.atan2(
self.initial_ecef_z,
math.sqrt(
math.pow(self.initial_ecef_x, 2) +
math.pow(self.initial_ecef_y, 2)))
self.ecef_to_ned_matrix = self.n_re(phiP, self.longitude0)
self.origin_enu_set = True
rospy.loginfo("Origin ENU frame set to: %.6f, %.6f, %.2f" %
(latitude, longitude, altitude))
def geodetic_to_ecef(self, latitude, longitude, altitude):
# Convert geodetic coordinates to ECEF.
# http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
lat_rad = math.radians(latitude)
lon_rad = math.radians(longitude)
xi = math.sqrt(1 - PiksiMulti.kFirstEccentricitySquared *
math.sin(lat_rad) * math.sin(lat_rad))
x = (PiksiMulti.kSemimajorAxis / xi +
altitude) * math.cos(lat_rad) * math.cos(lon_rad)
y = (PiksiMulti.kSemimajorAxis / xi +
altitude) * math.cos(lat_rad) * math.sin(lon_rad)
z = (PiksiMulti.kSemimajorAxis / xi *
(1 - PiksiMulti.kFirstEccentricitySquared) +
altitude) * math.sin(lat_rad)
return x, y, z
def ecef_to_ned(self, x, y, z):
# Converts ECEF coordinate position into local-tangent-plane NED.
# Coordinates relative to given ECEF coordinate frame.
vect = np.array([0.0, 0.0, 0.0])
vect[0] = x - self.initial_ecef_x
vect[1] = y - self.initial_ecef_y
vect[2] = z - self.initial_ecef_z
ret = self.ecef_to_ned_matrix.dot(vect)
n = ret[0]
e = ret[1]
d = -ret[2]
return n, e, d
def geodetic_to_enu(self, latitude, longitude, altitude):
# Geodetic position to local ENU frame
(x, y, z) = self.geodetic_to_ecef(latitude, longitude, altitude)
(north, east, down) = self.ecef_to_ned(x, y, z)
# Return East, North, Up coordinate.
return east, north, -down
def n_re(self, lat_radians, lon_radians):
s_lat = math.sin(lat_radians)
s_lon = math.sin(lon_radians)
c_lat = math.cos(lat_radians)
c_lon = math.cos(lon_radians)
ret = np.eye(3)
ret[0, 0] = -s_lat * c_lon
ret[0, 1] = -s_lat * s_lon
ret[0, 2] = c_lat
ret[1, 0] = -s_lon
ret[1, 1] = c_lon
ret[1, 2] = 0.0
ret[2, 0] = c_lat * c_lon
ret[2, 1] = c_lat * s_lon
ret[2, 2] = s_lat
return ret
def enu_to_pose_msg(self, east, north, up, variance):
pose_msg = PoseWithCovariance()
# Fill covariance using variance parameter of GPS.
pose_msg.covariance[6 * 0 + 0] = variance[0]
pose_msg.covariance[6 * 1 + 1] = variance[1]
pose_msg.covariance[6 * 2 + 2] = variance[2]
# Fill pose section.
pose_msg.pose.position.x = east
pose_msg.pose.position.y = north
pose_msg.pose.position.z = up
# GPS points do not have orientation
pose_msg.pose.orientation.x = 0.0
pose_msg.pose.orientation.y = 0.0
pose_msg.pose.orientation.z = 0.0
pose_msg.pose.orientation.w = 1.0
return pose_msg
def enu_to_point_msg(self, east, north, up):
point_msg = Point()
# Fill pose section.
point_msg.x = east
point_msg.y = north
point_msg.z = up
return point_msg
def enu_to_transform_msg(self, east, north, up):
transform_msg = Transform()
# Fill message.
transform_msg.translation.x = east
transform_msg.translation.y = north
transform_msg.translation.z = up
# Set orientation to unit quaternion as it does not really metter.
transform_msg.rotation.x = 0.0
transform_msg.rotation.y = 0.0
transform_msg.rotation.z = 0.0
transform_msg.rotation.w = 1.0
return transform_msg
def reset_piksi_service_callback(self, request):
response = std_srvs.srv.SetBoolResponse()
if request.data:
# Send reset message.
reset_sbp = SBP(SBP_MSG_RESET)
reset_sbp.payload = ''
reset_msg = reset_sbp.pack()
self.driver.write(reset_msg)
rospy.logwarn("Piksi hard reset via rosservice call.")
# Init messages with "memory".
self.receiver_state_msg = self.init_receiver_state_msg()
self.num_wifi_corrections = self.init_num_corrections_msg()
response.success = True
response.message = "Piksi reset command sent."
else:
response.success = False
response.message = "Piksi reset command not sent."
return response
def __init__(self):
# Print info.
rospy.sleep(
0.5) # Wait for a while for init to complete before printing.
rospy.loginfo(rospy.get_name() + " start")
rospy.loginfo("libsbp version currently used: " +
sbp.version.get_git_version())
# Check for correct SBP library version dependent on Piksi device.
if PiksiMulti.LIB_SBP_VERSION_MULTI != sbp.version.get_git_version():
rospy.logwarn(
"Lib SBP version in usage (%s) is different than the one used to test this driver (%s)!"
% (sbp.version.get_git_version(),
PiksiMulti.LIB_SBP_VERSION_MULTI))
# Open a connection to Piksi.
serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')
baud_rate = rospy.get_param('~baud_rate', 115200)
try:
self.driver = PySerialDriver(serial_port, baud=baud_rate)
except SystemExit:
rospy.logerr("Piksi not found on serial port '%s'", serial_port)
raise
# Create a handler to connect Piksi driver to callbacks.
self.framer = Framer(self.driver.read, self.driver.write, verbose=True)
self.handler = Handler(self.framer)
self.debug_mode = rospy.get_param('~debug_mode', False)
if self.debug_mode:
rospy.loginfo(
"Piksi driver started in debug mode, every available topic will be published."
)
else:
rospy.loginfo("Piksi driver started in normal mode.")
# Corrections over WiFi settings.
self.base_station_mode = rospy.get_param('~base_station_mode', False)
self.udp_broadcast_addr = rospy.get_param('~broadcast_addr',
'255.255.255.255')
self.udp_port = rospy.get_param('~broadcast_port', 26078)
self.base_station_ip_for_latency_estimation = rospy.get_param(
'~base_station_ip_for_latency_estimation', '10.10.10.1')
self.multicaster = []
self.multicast_recv = []
# Navsatfix settings.
self.var_spp = rospy.get_param('~var_spp', [25.0, 25.0, 64.0])
self.var_rtk_float = rospy.get_param('~var_rtk_float',
[25.0, 25.0, 64.0])
self.var_rtk_fix = rospy.get_param('~var_rtk_fix',
[0.0049, 0.0049, 0.01])
self.navsatfix_frame_id = rospy.get_param('~navsatfix_frame_id', 'gps')
# Local ENU frame settings.
self.origin_enu_set = False
self.latitude0 = 0.0
self.longitude0 = 0.0
self.altitude0 = 0.0
self.initial_ecef_x = 0.0
self.initial_ecef_y = 0.0
self.initial_ecef_z = 0.0
self.ecef_to_ned_matrix = np.eye(3)
self.enu_frame_id = rospy.get_param('~enu_frame_id', 'enu')
self.transform_child_frame_id = rospy.get_param(
'~transform_child_frame_id', 'gps_receiver')
if rospy.has_param('~latitude0_deg') and rospy.has_param(
'~longitude0_deg') and rospy.has_param('~altitude0'):
latitude0 = rospy.get_param('~latitude0_deg')
longitude0 = rospy.get_param('~longitude0_deg')
altitude0 = rospy.get_param('~altitude0')
# Set origin ENU frame to coordinate specified by rosparam.
self.init_geodetic_reference(latitude0, longitude0, altitude0)
rospy.loginfo("Origin ENU frame set by rosparam.")
# Advertise topics.
self.publishers = self.advertise_topics()
# Create callbacks.
self.create_callbacks()
# Init messages with "memory".
self.receiver_state_msg = self.init_receiver_state_msg()
self.num_wifi_corrections = self.init_num_corrections_msg()
# Corrections over wifi message, if we are not the base station.
if not self.base_station_mode:
# Start new thread to periodically ping base station.
threading.Thread(target=self.ping_base_station_over_wifi).start()
self.handler.start()
# Reset service.
self.reset_piksi_service = rospy.Service(
rospy.get_name() + '/reset_piksi', std_srvs.srv.SetBool,
self.reset_piksi_service_callback)
# Watchdog timer info
self.watchdog_time = rospy.get_rostime()
self.messages_started = False
# Only have start-up reset in base station mode
if self.base_station_mode:
# Things have 30 seconds to start or we will kill node
rospy.Timer(rospy.Duration(30), self.watchdog_callback, True)
# Spin.
rospy.spin()