class UDPReceiver(object):
def __init__(self, host, port, callback):
self._callback = callback
self.driver = UDPDriver(host, port)
self.framer = Framer(self.driver.read, None, verbose=False)
self.piksi = Handler(self.framer)
self.piksi.add_callback(self.callback)
self.piksi.start()
def callback(self, msg, **metadata):
self._callback(msg, **metadata)
class SbpUdpMulticastReceiver:
def __init__(self, port, ext_callback):
self._callback = ext_callback
self.driver = UDPDriver(' ', port)
self.framer = Framer(self.driver.read, None, verbose=False)
self.piksi = Handler(self.framer)
self.piksi.add_callback(self.recv_callback)
self.piksi.start()
def recv_callback(self, msg, **metadata):
self._callback(msg, **metadata)
class SerialReceiver(object):
def __init__(self, port, baud_rate, callback):
self._callback = callback
self.driver = PySerialDriver(port, baud=baud_rate)
self.framer = Framer(self.driver.read, None, verbose=False)
self.piksi = Handler(self.framer)
self.piksi.add_callback(self.callback)
self.piksi.start()
def callback(self, msg, **metadata):
self._callback(msg, **metadata)
class UDPReceiver(object):
def __init__(self, host, port, callback):
self._callback = callback
self.driver = UDPDriver(host, port)
self.framer = Framer(self.driver.read, None, verbose=False)
self.piksi = Handler(self.framer)
self.piksi.add_callback(self.callback)
self.piksi.start()
def callback(self, msg, **metadata):
self._callback(msg, **metadata)
class SerialReceiver(object):
def __init__(self, port, baud_rate, callback):
self._callback = callback
self.driver = PySerialDriver(port, baud=baud_rate)
self.framer = Framer(self.driver.read, None, verbose=False)
self.piksi = Handler(self.framer)
self.piksi.add_callback(self.callback)
self.piksi.start()
def callback(self, msg, **metadata):
self._callback(msg, **metadata)
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()
class GPS_Adapter:
"""Provide access to the latest data from a fake sensor."""
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 _update_gps_callback(self, msg, **metadata):
if msg.n_sats != 0:
with self.data_lock:
self.data = GPSData(msg.lat, msg.lon, msg.height)
def get_GPS_data(self, request, store_helper):
"""Save the latest GPS data to the data store."""
data_id = data_acquisition_pb2.DataIdentifier(
action_id=request.action_id, channel=kChannel)
# Get GPS data.
with self.data_lock:
data = self.data
# Populate data acquisition store message.
message = data_acquisition_pb2.AssociatedMetadata()
message.reference_id.action_id.CopyFrom(request.action_id)
message.metadata.data.update({
"latitude": data.lat,
"longitude": data.lon,
"altitude": data.alt,
})
_LOGGER.info("Retrieving GPS data: {}".format(message.metadata.data))
# Store the data and manage store state.
store_helper.store_metadata(message, data_id)
store_helper.state.set_status(
data_acquisition_pb2.GetStatusResponse.STATUS_SAVING)
# Generate publisher and callback function for navsatfix messages
if publish_spp or publish_rtk:
if publish_rtk:
pub_rtk_float = rospy.Publisher(rospy.get_name() + '/navsatfix_rtk_float', NavSatFix, queue_size = 10)
pub_rtk_fix = rospy.Publisher(rospy.get_name() + '/navsatfix_rtk_fix', NavSatFix, queue_size = 10)
if publish_spp:
pub_spp = rospy.Publisher(rospy.get_name() + '/navsatfix_spp', NavSatFix, queue_size = 10)
# Define fixed attributes of the NavSatFixed message
navsatfix_msg = NavSatFix()
navsatfix_msg.header.frame_id = rospy.get_param('~frame_id')
navsatfix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
navsatfix_msg.status.service = NavSatStatus.SERVICE_GPS
handler.add_callback(navsatfix_callback, msg_type=SBP_MSG_POS_LLH)
# Generate publisher and callback function for PiksiBaseline messages
if publish_piksibaseline:
pub_piksibaseline = rospy.Publisher(rospy.get_name() + '/piksibaseline', PiksiBaseline, queue_size = 10)
baseline_msg = PiksiBaseline()
handler.add_callback(baseline_callback, msg_type=SBP_MSG_BASELINE_NED)
# Initialize Navigation messages
init_callback_and_publisher('baseline_ecef', msg_baseline_ecef, SBP_MSG_BASELINE_ECEF, MsgBaselineECEF,
'tow', 'x', 'y', 'z', 'accuracy', 'n_sats', 'flags')
init_callback_and_publisher('baseline_ned', msg_baseline_ned, SBP_MSG_BASELINE_NED, MsgBaselineNED,
'tow', 'n', 'e', 'd', 'h_accuracy', 'v_accuracy', 'n_sats', 'flags')
init_callback_and_publisher('dops', msg_dops, SBP_MSG_DOPS, MsgDops, 'tow', 'gdop', 'pdop', 'tdop', 'hdop', 'vdop')
init_callback_and_publisher('gps_time', msg_gps_time, SBP_MSG_GPS_TIME, MsgGPSTime, 'wn', 'tow', 'ns', 'flags')
class PiksiROS(object):
def __init__(self):
rospy.init_node("piksi_ros")
self.send_observations = False
self.serial_number = None
self.last_baseline = None
self.last_vel = None
self.last_pos = None
self.last_dops = None
self.last_rtk_pos = None
self.last_spp_time = 0
self.last_rtk_time = 0
self.fix_mode = -1
self.rtk_fix_mode = 2
self.read_piksi_settings_info()
self.piksi_settings = tree()
self.proj = None
self.disconnect_piksi()
self.read_params()
self.setup_comms()
self.odom_msg = Odometry()
self.odom_msg.header.frame_id = self.rtk_frame_id
self.odom_msg.child_frame_id = self.child_frame_id
self.odom_msg.pose.pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
self.odom_msg.pose.covariance[0] = self.rtk_h_accuracy**2
self.odom_msg.pose.covariance[7] = self.rtk_h_accuracy**2
self.odom_msg.pose.covariance[14] = self.rtk_v_accuracy**2
self.odom_msg.twist.covariance[
0] = self.odom_msg.pose.covariance[0] * 4
self.odom_msg.twist.covariance[
7] = self.odom_msg.pose.covariance[7] * 4
self.odom_msg.twist.covariance[
14] = self.odom_msg.pose.covariance[14] * 4
self.odom_msg.pose.covariance[21] = COV_NOT_MEASURED
self.odom_msg.pose.covariance[28] = COV_NOT_MEASURED
self.odom_msg.pose.covariance[35] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[21] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[28] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[35] = COV_NOT_MEASURED
self.transform = TransformStamped()
self.transform.header.frame_id = self.utm_frame_id
self.transform.child_frame_id = self.rtk_frame_id
self.transform.transform.rotation = Quaternion(x=0, y=0, z=0, w=1)
# Used to publish transform from rtk_frame_id -> child_frame_id
self.base_link_transform = TransformStamped()
self.base_link_transform.header.frame_id = self.rtk_frame_id
self.base_link_transform.child_frame_id = self.child_frame_id
self.base_link_transform.transform.rotation = Quaternion(x=0,
y=0,
z=0,
w=1)
self.diag_updater = diagnostic_updater.Updater()
self.heartbeat_diag = diagnostic_updater.FrequencyStatus(
diagnostic_updater.FrequencyStatusParam(
{
'min': self.diag_heartbeat_freq,
'max': self.diag_heartbeat_freq
}, self.diag_freq_tolerance, self.diag_window_size))
self.diag_updater.add("Piksi status", self.diag)
self.diag_updater.add(self.heartbeat_diag)
self.setup_pubsub()
def read_piksi_settings_info(self):
self.piksi_settings_info = tree()
settings_info = yaml.load(
open(os.path.join(PKG_PATH, 'piksi_settings.yaml'), 'r'))
for s in settings_info:
if s['type'].lower() == 'boolean':
s['parser'] = lambda x: x.lower() == 'true'
elif s['type'].lower() in ('float', 'double', 'int'):
s['parser'] = ast.literal_eval
elif s['type'] == 'enum':
s['parser'] = s['enum'].index
else:
s['parser'] = lambda x: x
self.piksi_settings_info[s['group']][s['name']] = s
def init_proj(self, latlon):
self.proj = Proj(proj='utm',
zone=calculate_utm_zone(*latlon)[0],
ellps='WGS84')
def reconfig_callback(self, config, level):
for p, v in config.iteritems():
if p == 'groups':
continue
n = p.split('__')
if self.piksi_settings[n[1]][n[2]] != v:
i = self.piksi_settings_info[n[1]][n[2]]
p_val = v
if i['type'] == 'enum':
try:
p_val = i['enum'][v]
except:
p_val = i['enum'][0]
self.piksi_set(n[1], n[2], p_val)
self.piksi_settings[n[1]][n[2]] = v
return config
def read_params(self):
self.debug = rospy.get_param('~debug', False)
self.frame_id = rospy.get_param('~frame_id', "piksi")
self.rtk_frame_id = rospy.get_param('~rtk_frame_id', "rtk_gps")
self.utm_frame_id = rospy.get_param('~utm_frame_id', "utm")
self.child_frame_id = rospy.get_param('~child_frame_id', "base_link")
self.piksi_port = rospy.get_param('~port', 55555)
self.piksi_host = rospy.get_param('~host', "192.168.0.222")
self.publish_utm_rtk_tf = rospy.get_param('~publish_utm_rtk_tf', False)
self.publish_rtk_child_tf = rospy.get_param('~publish_rtk_child_tf',
False)
self.diag_heartbeat_freq = rospy.get_param('~diag/heartbeat_freq', 1.0)
self.diag_update_freq = rospy.get_param('~diag/update_freq', 10.0)
self.diag_freq_tolerance = rospy.get_param('~diag/freq_tolerance', 0.1)
self.diag_window_size = rospy.get_param('~diag/window_size', 10)
self.diag_min_delay = rospy.get_param('~diag/min_delay', 0.0)
self.diag_max_delay = rospy.get_param('~diag/max_delay', 0.2)
# Send/receive observations through udp
self.obs_udp_send = rospy.get_param('~obs/udp/send', False)
self.obs_udp_recv = rospy.get_param('~obs/udp/receive', False)
self.obs_udp_host = rospy.get_param('~obs/udp/host', "192.168.0.223")
self.obs_udp_port = rospy.get_param('~obs/udp/port', 55556)
# Send/receive observations through serial port
self.obs_serial_send = rospy.get_param('~obs/serial/send', False)
self.obs_serial_recv = rospy.get_param('~obs/serial/receive', False)
self.obs_serial_port = rospy.get_param('~obs/serial/port', None)
self.obs_serial_baud_rate = rospy.get_param('~obs/serial/baud_rate',
115200)
self.rtk_h_accuracy = rospy.get_param("~rtk_h_accuracy", 0.04)
self.rtk_v_accuracy = rospy.get_param("~rtk_v_accuracy",
self.rtk_h_accuracy * 3)
self.sbp_log = rospy.get_param('~sbp_log_file', None)
self.rtk_fix_timeout = rospy.get_param('~rtk_fix_timeout', 0.2)
self.spp_fix_timeout = rospy.get_param('~spp_fix_timeout', 1.0)
self.publish_ephemeris = rospy.get_param('~publish_ephemeris', False)
self.publish_observations = rospy.get_param('~publish_observations',
False)
self.piksi_update_settings = rospy.get_param('~piksi', {})
self.piksi_save_settings = rospy.get_param('~piksi_save_settings',
False)
def setup_comms(self):
self.obs_senders = []
self.obs_receivers = []
if self.obs_udp_send or self.obs_serial_send:
self.send_observations = True
if self.obs_udp_send:
self.obs_senders.append(
UDPSender(self.obs_udp_host, self.obs_udp_port))
if self.obs_serial_send:
self.obs_senders.append(
SerialSender(self.obs_serial_port, self.obs_serial_baud_rate))
if self.obs_udp_recv:
self.obs_receivers.append(
UDPReceiver(self.obs_udp_host, self.obs_udp_port,
self.callback_external))
if self.obs_serial_recv:
self.obs_receivers.append(
SerialReceiver(self.obs_serial_port, self.obs_serial_baud_rate,
self.callback_external))
def callback_external(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received external SBP msg.")
if self.piksi_framer:
self.piksi_framer(msg, **metadata)
else:
rospy.logwarn(
"Received external SBP msg, but Piksi not connected.")
def setup_pubsub(self):
freq_params = diagnostic_updater.FrequencyStatusParam(
{
'min': self.diag_update_freq,
'max': self.diag_update_freq
}, self.diag_freq_tolerance, self.diag_window_size)
time_params = diagnostic_updater.TimeStampStatusParam(
self.diag_min_delay, self.diag_max_delay)
self.pub_fix = rospy.Publisher("~fix", NavSatFix, queue_size=1000)
self.pub_spp_fix = diagnostic_updater.DiagnosedPublisher(
rospy.Publisher("~spp_fix", NavSatFix, queue_size=1000),
self.diag_updater, freq_params, time_params)
self.pub_rtk_fix = diagnostic_updater.DiagnosedPublisher(
rospy.Publisher("~rtk_fix", NavSatFix, queue_size=1000),
self.diag_updater, freq_params, time_params)
# self.pub_rtk = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params)
self.pub_odom = diagnostic_updater.DiagnosedPublisher(
rospy.Publisher("~odom", Odometry, queue_size=1000),
self.diag_updater, freq_params, time_params)
self.pub_time = diagnostic_updater.DiagnosedPublisher(
rospy.Publisher("~time", TimeReference, queue_size=1000),
self.diag_updater, freq_params, time_params)
if self.publish_utm_rtk_tf or self.publish_rtk_child_tf:
self.tf_br = tf2_ros.TransformBroadcaster()
if self.publish_ephemeris:
self.pub_eph = rospy.Publisher("~ephemeris",
Ephemeris,
queue_size=1000)
if self.publish_observations:
self.pub_obs = rospy.Publisher('~observations',
Observations,
queue_size=1000)
def piksi_start(self):
self.dr_srv = GroupServer(PiksiDriverConfig,
self.reconfig_callback,
ns_prefix=('dynamic_reconfigure', ),
nest_groups=False)
self.piksi.add_callback(self.callback_sbp_gps_time,
msg_type=SBP_MSG_GPS_TIME)
self.piksi.add_callback(self.callback_sbp_dops, msg_type=SBP_MSG_DOPS)
self.piksi.add_callback(self.callback_sbp_pos,
msg_type=SBP_MSG_POS_LLH)
self.piksi.add_callback(self.callback_sbp_baseline,
msg_type=SBP_MSG_BASELINE_NED)
self.piksi.add_callback(self.callback_sbp_vel,
msg_type=SBP_MSG_VEL_NED)
# if self.send_observations:
self.piksi.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
self.piksi.add_callback(self.callback_sbp_base_pos,
msg_type=SBP_MSG_BASE_POS_LLH)
if self.publish_ephemeris:
self.piksi.add_callback(self.callback_sbp_ephemeris,
msg_type=SBP_MSG_EPHEMERIS)
if self.sbp_log is not None:
self.sbp_logger = RotatingFileLogger(self.sbp_log,
when='M',
interval=60,
backupCount=0)
self.piksi.add_callback(self.sbp_logger)
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 disconnect_piksi(self):
try:
self.piksi.stop()
self.piksi.remove_callback()
except:
pass
self.piksi = None
self.piksi_framer = None
self.piksi_driver = None
def set_serial_number(self, serial_number):
rospy.loginfo("Connected to piksi #%d" % serial_number)
self.serial_number = serial_number
self.diag_updater.setHardwareID("Piksi %d" % serial_number)
def read_piksi_settings(self):
self.settings_index = 0
self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))
# self.piksi_framer(MsgSettingsReadReq(setting='simulator\0enabled\0'))
def piksi_set(self, section, setting, value):
m = MsgSettingsWrite(setting="%s\0%s\0%s\0" %
(section, setting, value))
self.piksi_framer(m)
def update_dr_param(self, section, name, value):
# print 'set %s:%s to %s' % (section, name, value)
# print '~dynamic_reconfigure/piksi__%s__%s' % (section, name), value
rospy.set_param('~dynamic_reconfigure/piksi__%s__%s' % (section, name),
value)
def set_piksi_settings(self):
save_needed = False
for s in nested_dict_iter(self.piksi_update_settings):
cval = self.piksi_settings[s[0][0]][s[0][1]]
if len(cval) != 0:
if cval != str(s[1]):
rospy.loginfo('Updating piksi setting %s:%s to %s.' %
(s[0][0], s[0][1], s[1]))
self.piksi_set(s[0][0], s[0][1], s[1])
self.update_dr_param(s[0][0], s[0][1], s[1])
save_needed = True
else:
rospy.loginfo('Piksi setting %s:%s already set to %s.' %
(s[0][0], s[0][1], s[1]))
else:
rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0]))
if self.piksi_save_settings and save_needed:
rospy.loginfo('Saving piksi settings to flash')
m = MsgSettingsSave()
self.piksi_framer(m)
def callback_sbp_startup(self, msg, **metadata):
rospy.loginfo('Piksi startup packet received: %s' % repr(msg))
def callback_sbp_gps_time(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" %
(msg.sender, repr(msg)))
out = TimeReference()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.time_ref = ros_time_from_sbp_time(msg)
out.source = "gps"
self.pub_time.publish(out)
def callback_sbp_heartbeat(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_HEARTBEAT (Sender: %d): %s" %
(msg.sender, repr(msg)))
self.heartbeat_diag.tick()
if self.serial_number is None:
self.set_serial_number(msg.sender)
self.read_piksi_settings()
def callback_sbp_dops(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_DOPS (Sender: %d): %s" %
(msg.sender, repr(msg)))
self.last_dops = msg
def callback_sbp_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" %
(msg.sender, repr(msg)))
out = NavSatFix()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.status.service = NavSatStatus.SERVICE_GPS
out.latitude = msg.lat
out.longitude = msg.lon
out.altitude = msg.height
out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
#TODO
if msg.flags & 0x07:
self.last_rtk_time = time.time()
self.rtk_fix_mode = msg.flags & 0x07
out.status.status = NavSatStatus.STATUS_GBAS_FIX #TODO
# TODO this should probably also include covariance of base fix?
out.position_covariance[
0] = msg.h_accuracy**2 #self.rtk_h_accuracy ** 2
out.position_covariance[
4] = msg.h_accuracy**2 #self.rtk_h_accuracy ** 2
out.position_covariance[
8] = msg.v_accuracy**2 #self.rtk_v_accuracy ** 2
pub = self.pub_rtk_fix
self.last_rtk_pos = msg
# If we are getting this message, RTK is our best fix, so publish this as our best fix.
self.pub_fix.publish(out)
else:
self.last_spp_time = time.time()
out.status.status = NavSatStatus.STATUS_FIX
# TODO hack, piksi should provide these numbers
if self.last_dops:
out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
else:
out.position_covariance[0] = COV_NOT_MEASURED
out.position_covariance[4] = COV_NOT_MEASURED
out.position_covariance[8] = COV_NOT_MEASURED
pub = self.pub_spp_fix
self.last_pos = msg
# Check if SPP is currently our best available fix
if self.rtk_fix_mode <= 0:
self.pub_fix.publish(out)
pub.publish(out)
def publish_odom(self):
if self.last_baseline is None or self.last_vel is None:
return
if self.last_baseline.tow == self.last_vel.tow:
self.odom_msg.header.stamp = rospy.Time.now()
self.odom_msg.pose.pose.position.x = self.last_baseline.e / 1000.0
self.odom_msg.pose.pose.position.y = self.last_baseline.n / 1000.0
self.odom_msg.pose.pose.position.z = -self.last_baseline.d / 1000.0
self.odom_msg.twist.twist.linear.x = self.last_vel.e / 1000.0
self.odom_msg.twist.twist.linear.y = self.last_vel.n / 1000.0
self.odom_msg.twist.twist.linear.z = -self.last_vel.d / 1000.0
self.pub_odom.publish(self.odom_msg)
def callback_sbp_vel(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_VEL_NED (Sender: %d): %s" %
(msg.sender, repr(msg)))
self.last_vel = msg
self.publish_odom()
def callback_sbp_baseline(self, msg, **metadata):
"""
:type msg: SBP_MSG_BASELINE_NED
"""
if self.debug:
rospy.loginfo("Received SBP_MSG_BASELINE_NED (Sender: %d): %s" %
(msg.sender, repr(msg)))
if self.publish_rtk_child_tf:
self.base_link_transform.header.stamp = rospy.Time.now()
self.base_link_transform.transform.translation.x = msg.e / 1000.0
self.base_link_transform.transform.translation.y = msg.n / 1000.0
self.base_link_transform.transform.translation.z = -msg.d / 1000.0
self.tf_br.sendTransform(self.base_link_transform)
self.last_baseline = msg
self.publish_odom()
def callback_sbp_ephemeris(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_EPHEMERIS (Sender: %d): %s" %
(msg.sender, repr(msg)))
if not hasattr(self, 'eph_msg'):
self.eph_msg = Ephemeris()
self.eph_msg.header.stamp = rospy.Time.now()
self.eph_msg.header.frame_id = self.frame_id
self.eph_msg.tgd = msg.tgd
self.eph_msg.c_rs = msg.c_rs
self.eph_msg.c_rc = msg.c_rc
self.eph_msg.c_uc = msg.c_uc
self.eph_msg.c_us = msg.c_us
self.eph_msg.c_ic = msg.c_ic
self.eph_msg.c_is = msg.c_is
self.eph_msg.dn = msg.dn
self.eph_msg.m0 = msg.m0
self.eph_msg.ecc = msg.ecc
self.eph_msg.sqrta = msg.sqrta
self.eph_msg.omega0 = msg.omega0
self.eph_msg.omegadot = msg.omegadot
self.eph_msg.w = msg.w
self.eph_msg.inc = msg.inc
self.eph_msg.inc_dot = msg.inc_dot
self.eph_msg.af0 = msg.af0
self.eph_msg.af1 = msg.af1
self.eph_msg.af2 = msg.af2
self.eph_msg.toe_tow = msg.toe_tow
self.eph_msg.toe_wn = msg.toe_wn
self.eph_msg.toc_tow = msg.toc_tow
self.eph_msg.toc_wn = msg.toc_wn
self.eph_msg.valid = msg.valid
self.eph_msg.healthy = msg.healthy
self.eph_msg.sid.sat = msg.sid.sat
self.eph_msg.sid.band = msg.sid.band
self.eph_msg.sid.constellation = msg.sid.constellation
self.eph_msg.iode = msg.iode
self.eph_msg.iodc = msg.iodc
self.pub_eph.publish(m)
def callback_sbp_obs(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_OBS (Sender: %d): %s" %
(msg.sender, repr(msg)))
if self.send_observations:
for s in self.obs_senders:
s.send(msg)
if self.publish_observations:
if not hasattr(self, 'obs_msg'):
self.obs_msg = Observations()
# Need to do some accounting to figure out how many sbp packets to expect
num_packets = (msg.header.n_obs >> 4) & 0x0F
packet_no = msg.header.n_obs & 0x0F
if self.obs_msg.tow != msg.header.t.tow:
self.obs_msg.header.stamp = rospy.Time.now()
self.obs_msg.header.frame_id = self.frame_id
self.obs_msg.tow = msg.header.t.tow
self.obs_msg.wn = msg.header.t.wn
self.obs_msg.n_obs = 0
self.obs_msg.obs = []
# lets use this field to count how many packets we have so far
self.obs_msg.n_obs += 1
for obs in msg.obs:
x = Obs()
x.P = obs.P
x.L.i = obs.L.i
x.L.f = obs.L.f
x.cn0 = obs.cn0
x.lock = obs.lock
x.sid.sat = obs.sid.sat
x.sid.band = obs.sid.band
x.sid.constellation = obs.sid.constellation
self.obs_msg.obs.append(x)
if num_packets == self.obs_msg.n_obs:
# Now use the field to indicate how many observations we have
self.obs_msg.n_obs = len(self.obs_msg.obs)
self.pub_obs.publish(self.obs_msg)
def callback_sbp_base_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_BASE_POS_LLH (Sender: %d): %s" %
(msg.sender, repr(msg)))
if self.send_observations:
for s in self.obs_senders:
s.send(msg)
# publish tf for rtk frame
if self.publish_utm_rtk_tf:
if not self.proj:
self.init_proj((msg.lat, msg.lon))
E, N = self.proj(msg.lon, msg.lat, inverse=False)
self.transform.header.stamp = rospy.Time.now()
self.transform.transform.translation.x = E
self.transform.transform.translation.y = N
self.transform.transform.translation.z = -msg.height
self.tf_br.sendTransform(self.transform)
def callback_sbp_settings_read_resp(self, msg, **metadata):
pass
def callback_sbp_settings_read_by_index_resp(self, msg, **metadata):
p = msg.setting.split('\0')
try:
i = self.piksi_settings_info[p[0]][p[1]]
p[2] = i['parser'](p[2])
except:
pass
rospy.set_param('~piksi_original_settings/%s/%s' % (p[0], p[1]), p[2])
self.piksi_settings[p[0]][p[1]] = p[2]
self.update_dr_param(p[0], p[1], p[2])
self.settings_index += 1
self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))
def callback_sbp_settings_read_by_index_done(self, msg, **metadata):
self.set_piksi_settings()
self.piksi_start()
def callback_sbp_log(self, msg, **metadata):
PIKSI_LOG_LEVELS_TO_ROS[msg.level]("Piksi LOG: %s" % msg.text)
def check_timeouts(self):
if time.time() - self.last_rtk_time > self.rtk_fix_timeout:
if time.time() - self.last_spp_time > self.spp_fix_timeout:
self.fix_mode = -1
else:
self.fix_mode = 0
else:
self.fix_mode = self.rtk_fix_mode
def diag(self, stat):
fix_mode = self.fix_mode
num_sats = 0
last_pos = None
if self.last_rtk_pos is not None:
last_pos = self.last_rtk_pos
elif self.last_pos is not None:
last_pos = self.last_pos
if last_pos:
stat.add("GPS latitude", last_pos.lat)
stat.add("GPS longitude", last_pos.lon)
stat.add("GPS altitude", last_pos.height)
stat.add("GPS #sats", last_pos.n_sats)
num_sats = last_pos.n_sats
stat.add("Height mode", HEIGHT_MODE[(last_pos.n_sats & 0x04) >> 2])
stat.add("RAIM availability",
RAIM_AVAILABILITY[(last_pos.n_sats & 0x08) >> 3])
stat.add("RAIM repair", RAIM_REPAIR[(last_pos.n_sats & 0x10) >> 4])
stat.add("Fix mode", FIX_MODE[self.fix_mode])
if self.last_vel:
stat.add("Velocity N", self.last_vel.n * 1E-3)
stat.add("Velocity E", self.last_vel.e * 1E-3)
stat.add("Velocity D", self.last_vel.d * 1E-3)
stat.add("Velocity #sats", self.last_vel.n_sats)
if self.last_baseline:
stat.add("Baseline N", self.last_baseline.n * 1E-3)
stat.add("Baseline E", self.last_baseline.e * 1E-3)
stat.add("Baseline D", self.last_baseline.d * 1E-3)
stat.add("Baseline #sats", self.last_baseline.n_sats)
stat.add("Baseline fix mode",
FIX_MODE[self.last_baseline.flags & 0x07])
if self.last_dops:
stat.add("GDOP", self.last_dops.gdop * 1E-2)
stat.add("PDOP", self.last_dops.pdop * 1E-2)
stat.add("TDOP", self.last_dops.tdop * 1E-2)
stat.add("HDOP", self.last_dops.hdop * 1E-2)
stat.add("VDOP", self.last_dops.vdop * 1E-2)
if not self.piksi:
stat.summary(DiagnosticStatus.ERROR, "Piksi not connected")
elif fix_mode < 0:
stat.summary(DiagnosticStatus.ERROR, "No fix")
elif fix_mode < 1:
stat.summary(DiagnosticStatus.WARN, "No RTK fix")
elif num_sats < 5:
stat.summary(DiagnosticStatus.WARN,
"Low number of satellites in view")
else:
stat.summary(DiagnosticStatus.OK, "Piksi connected")
def spin(self):
reconnect_delay = 1.0
while not rospy.is_shutdown():
try:
rospy.loginfo("Connecting to SwiftNav Piksi on %s:%s" %
(self.piksi_host, self.piksi_port))
self.connect_piksi()
while not rospy.is_shutdown():
rospy.sleep(0.05)
if not self.piksi.is_alive():
raise IOError
self.diag_updater.update()
self.check_timeouts()
break # should only happen if rospy is trying to shut down
except IOError as e:
rospy.logerr("IOError")
self.disconnect_piksi()
except SystemExit as e:
rospy.logerr("Unable to connect to Piksi on port %s" %
self.piksi_port)
self.disconnect_piksi()
except: # catch *all* exceptions
e = sys.exc_info()[0]
rospy.logerr("Uncaught error: %s" % repr(e))
self.disconnect_piksi()
rospy.loginfo("Attempting to reconnect in %fs" % reconnect_delay)
rospy.sleep(reconnect_delay)
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
class PiksiROS(object):
def __init__(self):
rospy.init_node("piksi_ros")
self.send_observations = False
self.serial_number = None
self.last_baseline = None
self.last_vel = None
self.last_pos = None
self.last_dops = None
self.last_rtk_pos = None
self.last_spp_time = 0
self.last_rtk_time = 0
self.fix_mode = -1
self.rtk_fix_mode = 2
self.read_piksi_settings_info()
self.piksi_settings = tree()
self.proj = None
self.disconnect_piksi()
self.read_params()
self.setup_comms()
self.odom_msg = Odometry()
self.odom_msg.header.frame_id = self.rtk_frame_id
self.odom_msg.child_frame_id = self.child_frame_id
self.odom_msg.pose.covariance[0] = self.rtk_h_accuracy**2
self.odom_msg.pose.covariance[7] = self.rtk_h_accuracy**2
self.odom_msg.pose.covariance[14] = self.rtk_v_accuracy**2
self.odom_msg.twist.covariance[0] = self.odom_msg.pose.covariance[0] * 4
self.odom_msg.twist.covariance[7] = self.odom_msg.pose.covariance[7] * 4
self.odom_msg.twist.covariance[14] = self.odom_msg.pose.covariance[14] * 4
self.odom_msg.pose.covariance[21] = COV_NOT_MEASURED
self.odom_msg.pose.covariance[28] = COV_NOT_MEASURED
self.odom_msg.pose.covariance[35] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[21] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[28] = COV_NOT_MEASURED
self.odom_msg.twist.covariance[35] = COV_NOT_MEASURED
self.transform = TransformStamped()
self.transform.header.frame_id = self.utm_frame_id
self.transform.child_frame_id = self.rtk_frame_id
self.transform.transform.rotation = Quaternion(x=0,y=0,z=0,w=1)
# Used to publish transform from rtk_frame_id -> child_frame_id
self.base_link_transform = TransformStamped()
self.base_link_transform.header.frame_id = self.rtk_frame_id
self.base_link_transform.child_frame_id = self.child_frame_id
self.base_link_transform.transform.rotation = Quaternion(x=0,y=0,z=0,w=1)
self.diag_updater = diagnostic_updater.Updater()
self.heartbeat_diag = diagnostic_updater.FrequencyStatus(diagnostic_updater.FrequencyStatusParam({'min':self.diag_heartbeat_freq, 'max':self.diag_heartbeat_freq}, self.diag_freq_tolerance, self.diag_window_size))
self.diag_updater.add("Piksi status", self.diag)
self.diag_updater.add(self.heartbeat_diag)
self.setup_pubsub()
def read_piksi_settings_info(self):
self.piksi_settings_info = tree()
settings_info = yaml.load(open(os.path.join(PKG_PATH, 'piksi_settings.yaml'), 'r'))
for s in settings_info:
if s['type'].lower() == 'boolean':
s['parser'] = lambda x: x.lower()=='true'
elif s['type'].lower() in ('float', 'double','int'):
s['parser'] = ast.literal_eval
elif s['type'] == 'enum':
s['parser'] = s['enum'].index
else:
s['parser'] = lambda x: x
self.piksi_settings_info[s['group']][s['name']] = s
def init_proj(self, latlon):
self.proj = Proj(proj='utm',zone=calculate_utm_zone(*latlon)[0],ellps='WGS84')
def reconfig_callback(self, config, level):
for p,v in config.iteritems():
if p=='groups':
continue
n = p.split('__')
if self.piksi_settings[n[1]][n[2]] != v:
i = self.piksi_settings_info[n[1]][n[2]]
p_val = v
if i['type'] == 'enum':
try:
p_val = i['enum'][v]
except:
p_val = i['enum'][0]
self.piksi_set(n[1],n[2],p_val)
self.piksi_settings[n[1]][n[2]] = v
return config
def read_params(self):
self.debug = rospy.get_param('~debug', False)
self.frame_id = rospy.get_param('~frame_id', "piksi")
self.rtk_frame_id = rospy.get_param('~rtk_frame_id', "rtk_gps")
self.utm_frame_id = rospy.get_param('~utm_frame_id', "utm")
self.child_frame_id = rospy.get_param('~child_frame_id', "base_link")
self.piksi_port = rospy.get_param('~port', "/dev/ttyUSB0")
self.publish_utm_rtk_tf = rospy.get_param('~publish_utm_rtk_tf', False)
self.publish_rtk_child_tf = rospy.get_param('~publish_rtk_child_tf', False)
self.diag_heartbeat_freq = rospy.get_param('~diag/heartbeat_freq', 1.0)
self.diag_update_freq = rospy.get_param('~diag/update_freq', 10.0)
self.diag_freq_tolerance = rospy.get_param('~diag/freq_tolerance', 0.1)
self.diag_window_size = rospy.get_param('~diag/window_size', 10)
self.diag_min_delay = rospy.get_param('~diag/min_delay', 0.0)
self.diag_max_delay = rospy.get_param('~diag/max_delay', 0.2)
# Send/receive observations through udp
self.obs_udp_send = rospy.get_param('~obs/udp/send', False)
self.obs_udp_recv = rospy.get_param('~obs/udp/receive', False)
self.obs_udp_host = rospy.get_param('~obs/udp/host', "")
self.obs_udp_port = rospy.get_param('~obs/udp/port', 50785)
# Send/receive observations through serial port
self.obs_serial_send = rospy.get_param('~obs/serial/send', False)
self.obs_serial_recv = rospy.get_param('~obs/serial/receive', False)
self.obs_serial_port = rospy.get_param('~obs/serial/port', None)
self.obs_serial_baud_rate = rospy.get_param('~obs/serial/baud_rate', 115200)
self.rtk_h_accuracy = rospy.get_param("~rtk_h_accuracy", 0.04)
self.rtk_v_accuracy = rospy.get_param("~rtk_v_accuracy", self.rtk_h_accuracy*3)
self.sbp_log = rospy.get_param('~sbp_log_file', None)
self.rtk_fix_timeout = rospy.get_param('~rtk_fix_timeout', 0.2)
self.spp_fix_timeout = rospy.get_param('~spp_fix_timeout', 1.0)
self.publish_ephemeris = rospy.get_param('~publish_ephemeris', False)
self.publish_observations = rospy.get_param('~publish_observations', False)
self.piksi_update_settings = rospy.get_param('~piksi',{})
self.piksi_save_settings = rospy.get_param('~piksi_save_settings', False)
def setup_comms(self):
self.obs_senders = []
self.obs_receivers = []
if self.obs_udp_send or self.obs_serial_send:
self.send_observations = True
if self.obs_udp_send:
self.obs_senders.append(UDPSender(self.obs_udp_host, self.obs_udp_port))
if self.obs_serial_send:
self.obs_senders.append(SerialSender(self.obs_serial_port, self.obs_serial_baud_rate))
if self.obs_udp_recv:
self.obs_receivers.append(UDPReceiver(self.obs_udp_host, self.obs_udp_port, self.callback_external))
if self.obs_serial_recv:
self.obs_receivers.append(SerialReceiver(self.obs_serial_port, self.obs_serial_baud_rate, self.callback_external))
def callback_external(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received external SBP msg.")
if self.piksi_framer:
self.piksi_framer(msg, **metadata)
else:
rospy.logwarn("Received external SBP msg, but Piksi not connected.")
def setup_pubsub(self):
freq_params = diagnostic_updater.FrequencyStatusParam({'min':self.diag_update_freq, 'max':self.diag_update_freq}, self.diag_freq_tolerance, self.diag_window_size)
time_params = diagnostic_updater.TimeStampStatusParam(self.diag_min_delay, self.diag_max_delay)
self.pub_fix = rospy.Publisher("~fix", NavSatFix, queue_size=1000)
self.pub_spp_fix = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~spp_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params)
self.pub_rtk_fix = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_fix", NavSatFix, queue_size=1000), self.diag_updater, freq_params, time_params)
#self.pub_rtk = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~rtk_odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params)
self.pub_odom = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~odom", Odometry, queue_size=1000), self.diag_updater, freq_params, time_params)
self.pub_time = diagnostic_updater.DiagnosedPublisher(rospy.Publisher("~time", TimeReference, queue_size=1000), self.diag_updater, freq_params, time_params)
if self.publish_utm_rtk_tf or self.publish_rtk_child_tf:
self.tf_br = tf2_ros.TransformBroadcaster()
if self.publish_ephemeris:
self.pub_eph = rospy.Publisher("~ephemeris", Ephemeris, queue_size=1000)
if self.publish_observations:
self.pub_obs = rospy.Publisher('~observations', Observations, queue_size=1000)
def piksi_start(self):
self.dr_srv = GroupServer(PiksiDriverConfig, self.reconfig_callback, ns_prefix=('dynamic_reconfigure',), nest_groups=False)
self.piksi.add_callback(self.callback_sbp_gps_time, msg_type=SBP_MSG_GPS_TIME)
self.piksi.add_callback(self.callback_sbp_dops, msg_type=SBP_MSG_DOPS)
self.piksi.add_callback(self.callback_sbp_pos, msg_type=SBP_MSG_POS_LLH)
self.piksi.add_callback(self.callback_sbp_baseline, msg_type=SBP_MSG_BASELINE_NED)
self.piksi.add_callback(self.callback_sbp_vel, msg_type=SBP_MSG_VEL_NED)
#if self.send_observations:
self.piksi.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
self.piksi.add_callback(self.callback_sbp_base_pos, msg_type=SBP_MSG_BASE_POS)
if self.publish_ephemeris:
self.piksi.add_callback(self.callback_sbp_ephemeris, msg_type=SBP_MSG_EPHEMERIS)
if self.sbp_log is not None:
self.sbp_logger = RotatingFileLogger(self.sbp_log, when='M', interval=60, backupCount=0)
self.piksi.add_callback(self.sbp_logger)
def connect_piksi(self):
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 disconnect_piksi(self):
try:
self.piksi.stop()
self.piksi.remove_callback()
except:
pass
self.piksi = None
self.piksi_framer = None
self.piksi_driver = None
def set_serial_number(self, serial_number):
rospy.loginfo("Connected to piksi #%d" % serial_number)
self.serial_number = serial_number
self.diag_updater.setHardwareID("Piksi %d" % serial_number)
def read_piksi_settings(self):
self.settings_index = 0
self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))
#self.piksi_framer(MsgSettingsReadReq(setting='simulator\0enabled\0'))
def piksi_set(self, section, setting, value):
m = MsgSettingsWrite(setting="%s\0%s\0%s\0" % (section, setting, value))
self.piksi_framer(m)
def update_dr_param(self, section, name, value):
#print 'set %s:%s to %s' % (section, name, value)
#print '~dynamic_reconfigure/piksi__%s__%s' % (section, name), value
rospy.set_param('~dynamic_reconfigure/piksi__%s__%s' % (section, name), value)
def set_piksi_settings(self):
save_needed = False
for s in nested_dict_iter(self.piksi_update_settings):
cval = self.piksi_settings[s[0][0]][s[0][1]]
if len(cval) != 0:
if cval != str(s[1]):
rospy.loginfo('Updating piksi setting %s:%s to %s.' % (s[0][0], s[0][1], s[1]))
self.piksi_set(s[0][0], s[0][1], s[1])
self.update_dr_param(s[0][0], s[0][1], s[1])
save_needed = True
else:
rospy.loginfo('Piksi setting %s:%s already set to %s.' % (s[0][0], s[0][1], s[1]))
else:
rospy.logwarn('Invalid piksi setting: %s' % ':'.join(s[0]))
if self.piksi_save_settings and save_needed:
rospy.loginfo('Saving piksi settings to flash')
m = MsgSettingsSave()
self.piksi_framer(m)
def callback_sbp_startup(self, msg, **metadata):
rospy.loginfo('Piksi startup packet received: %s' % repr(msg))
def callback_sbp_gps_time(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_GPS_TIME (Sender: %d): %s" % (msg.sender, repr(msg)))
out = TimeReference()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.time_ref = ros_time_from_sbp_time(msg)
out.source = "gps"
self.pub_time.publish(out)
def callback_sbp_heartbeat(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_HEARTBEAT (Sender: %d): %s" % (msg.sender, repr(msg)))
self.heartbeat_diag.tick()
if self.serial_number is None:
self.set_serial_number(msg.sender)
self.read_piksi_settings()
def callback_sbp_dops(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_DOPS (Sender: %d): %s" % (msg.sender, repr(msg)))
self.last_dops = msg
def callback_sbp_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_POS_LLH (Sender: %d): %s" % (msg.sender, repr(msg)))
out = NavSatFix()
out.header.frame_id = self.frame_id
out.header.stamp = rospy.Time.now()
out.status.service = NavSatStatus.SERVICE_GPS
out.latitude = msg.lat
out.longitude = msg.lon
out.altitude = msg.height
out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
if msg.flags & 0x03:
self.last_rtk_time = time.time()
self.rtk_fix_mode = msg.flags & 0x03
out.status.status = NavSatStatus.STATUS_GBAS_FIX
# TODO this should probably also include covariance of base fix?
out.position_covariance[0] = self.rtk_h_accuracy**2
out.position_covariance[4] = self.rtk_h_accuracy**2
out.position_covariance[8] = self.rtk_v_accuracy**2
pub = self.pub_rtk_fix
self.last_rtk_pos = msg
# If we are getting this message, RTK is our best fix, so publish this as our best fix.
self.pub_fix.publish(out)
else:
self.last_spp_time = time.time()
out.status.status = NavSatStatus.STATUS_FIX
# TODO hack, piksi should provide these numbers
if self.last_dops:
out.position_covariance[0] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[4] = (self.last_dops.hdop * 1E-2)**2
out.position_covariance[8] = (self.last_dops.vdop * 1E-2)**2
else:
out.position_covariance[0] = COV_NOT_MEASURED
out.position_covariance[4] = COV_NOT_MEASURED
out.position_covariance[8] = COV_NOT_MEASURED
pub = self.pub_spp_fix
self.last_pos = msg
# Check if SPP is currently our best available fix
if self.rtk_fix_mode <= 0:
self.pub_fix.publish(out)
pub.publish(out)
def publish_odom(self):
if self.last_baseline is None or self.last_vel is None:
return
if self.last_baseline.tow == self.last_vel.tow:
self.odom_msg.header.stamp = rospy.Time.now()
self.odom_msg.pose.pose.position.x = self.last_baseline.e/1000.0
self.odom_msg.pose.pose.position.y = self.last_baseline.n/1000.0
self.odom_msg.pose.pose.position.z = -self.last_baseline.d/1000.0
self.odom_msg.twist.twist.linear.x = self.last_vel.e/1000.0
self.odom_msg.twist.twist.linear.y = self.last_vel.n/1000.0
self.odom_msg.twist.twist.linear.z = -self.last_vel.d/1000.0
self.pub_odom.publish(self.odom_msg)
def callback_sbp_vel(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_VEL_NED (Sender: %d): %s" % (msg.sender, repr(msg)))
self.last_vel = msg
self.publish_odom()
def callback_sbp_baseline(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_BASELINE_NED (Sender: %d): %s" % (msg.sender, repr(msg)))
if self.publish_rtk_child_tf:
self.base_link_transform.header.stamp = rospy.Time.now()
self.base_link_transform.transform.translation.x = msg.e/1000.0
self.base_link_transform.transform.translation.y = msg.n/1000.0
self.base_link_transform.transform.translation.z = -msg.d/1000.0
self.tf_br.sendTransform(self.base_link_transform)
self.last_baseline = msg
self.publish_odom()
def callback_sbp_ephemeris(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_EPHEMERIS (Sender: %d): %s" % (msg.sender, repr(msg)))
if not hasattr(self, 'eph_msg'):
self.eph_msg = Ephemeris()
self.eph_msg.header.stamp = rospy.Time.now()
self.eph_msg.header.frame_id = self.frame_id
self.eph_msg.tgd = msg.tgd
self.eph_msg.c_rs = msg.c_rs
self.eph_msg.c_rc = msg.c_rc
self.eph_msg.c_uc = msg.c_uc
self.eph_msg.c_us = msg.c_us
self.eph_msg.c_ic = msg.c_ic
self.eph_msg.c_is = msg.c_is
self.eph_msg.dn = msg.dn
self.eph_msg.m0 = msg.m0
self.eph_msg.ecc = msg.ecc
self.eph_msg.sqrta = msg.sqrta
self.eph_msg.omega0 = msg.omega0
self.eph_msg.omegadot = msg.omegadot
self.eph_msg.w = msg.w
self.eph_msg.inc = msg.inc
self.eph_msg.inc_dot = msg.inc_dot
self.eph_msg.af0 = msg.af0
self.eph_msg.af1 = msg.af1
self.eph_msg.af2 = msg.af2
self.eph_msg.toe_tow = msg.toe_tow
self.eph_msg.toe_wn = msg.toe_wn
self.eph_msg.toc_tow = msg.toc_tow
self.eph_msg.toc_wn = msg.toc_wn
self.eph_msg.valid = msg.valid
self.eph_msg.healthy = msg.healthy
self.eph_msg.sid.sat = msg.sid.sat
self.eph_msg.sid.band = msg.sid.band
self.eph_msg.sid.constellation = msg.sid.constellation
self.eph_msg.iode = msg.iode
self.eph_msg.iodc = msg.iodc
self.pub_eph.publish(m)
def callback_sbp_obs(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_OBS (Sender: %d): %s" % (msg.sender, repr(msg)))
if self.send_observations:
for s in self.obs_senders:
s.send(msg)
if self.publish_observations:
if not hasattr(self, 'obs_msg'):
self.obs_msg = Observations()
# Need to do some accounting to figure out how many sbp packets to expect
num_packets = (msg.header.n_obs >> 4) & 0x0F
packet_no = msg.header.n_obs & 0x0F
if self.obs_msg.tow != msg.header.t.tow:
self.obs_msg.header.stamp = rospy.Time.now()
self.obs_msg.header.frame_id = self.frame_id
self.obs_msg.tow = msg.header.t.tow
self.obs_msg.wn = msg.header.t.wn
self.obs_msg.n_obs = 0
self.obs_msg.obs = []
# lets use this field to count how many packets we have so far
self.obs_msg.n_obs += 1
for obs in msg.obs:
x = Obs()
x.P = obs.P
x.L.i = obs.L.i
x.L.f = obs.L.f
x.cn0 = obs.cn0
x.lock = obs.lock
x.sid.sat = obs.sid.sat
x.sid.band = obs.sid.band
x.sid.constellation = obs.sid.constellation
self.obs_msg.obs.append(x)
if num_packets == self.obs_msg.n_obs:
# Now use the field to indicate how many observations we have
self.obs_msg.n_obs = len(self.obs_msg.obs)
self.pub_obs.publish(self.obs_msg)
def callback_sbp_base_pos(self, msg, **metadata):
if self.debug:
rospy.loginfo("Received SBP_MSG_BASE_POS (Sender: %d): %s" % (msg.sender, repr(msg)))
if self.send_observations:
for s in self.obs_senders:
s.send(msg)
# publish tf for rtk frame
if self.publish_utm_rtk_tf:
if not self.proj:
self.init_proj((msg.lat, msg.lon))
E,N = self.proj(msg.lon,msg.lat, inverse=False)
self.transform.header.stamp = rospy.Time.now()
self.transform.transform.translation.x = E
self.transform.transform.translation.y = N
self.transform.transform.translation.z = -msg.height
self.tf_br.sendTransform(self.transform)
def callback_sbp_settings_read_resp(self, msg, **metadata):
pass
def callback_sbp_settings_read_by_index_resp(self, msg, **metadata):
p = msg.setting.split('\0')
try:
i = self.piksi_settings_info[p[0]][p[1]]
p[2] = i['parser'](p[2])
except:
pass
rospy.set_param('~piksi_original_settings/%s/%s' % (p[0],p[1]), p[2])
self.piksi_settings[p[0]][p[1]] = p[2]
self.update_dr_param(p[0], p[1], p[2])
self.settings_index += 1
self.piksi_framer(MsgSettingsReadByIndexReq(index=self.settings_index))
def callback_sbp_settings_read_by_index_done(self, msg, **metadata):
self.set_piksi_settings()
self.piksi_start()
def callback_sbp_log(self, msg, **metadata):
PIKSI_LOG_LEVELS_TO_ROS[msg.level]("Piksi LOG: %s" % msg.text)
def check_timeouts(self):
if time.time() - self.last_rtk_time > self.rtk_fix_timeout:
if time.time() - self.last_spp_time > self.spp_fix_timeout:
self.fix_mode = -1
else:
self.fix_mode = 0
else:
self.fix_mode = self.rtk_fix_mode
def diag(self, stat):
fix_mode = self.fix_mode
num_sats = 0
last_pos = None
if self.last_rtk_pos is not None:
last_pos = self.last_rtk_pos
elif self.last_pos is not None:
last_pos = self.last_pos
if last_pos:
stat.add("GPS latitude", last_pos.lat)
stat.add("GPS longitude", last_pos.lon)
stat.add("GPS altitude", last_pos.height)
stat.add("GPS #sats", last_pos.n_sats)
num_sats = last_pos.n_sats
stat.add("Height mode", HEIGHT_MODE[(last_pos.n_sats & 0x04) >> 2])
stat.add("RAIM availability", RAIM_AVAILABILITY[(last_pos.n_sats & 0x08) >> 3])
stat.add("RAIM repair", RAIM_REPAIR[(last_pos.n_sats & 0x10) >> 4])
stat.add("Fix mode", FIX_MODE[self.fix_mode])
if self.last_vel:
stat.add("Velocity N", self.last_vel.n * 1E-3)
stat.add("Velocity E", self.last_vel.e * 1E-3)
stat.add("Velocity D", self.last_vel.d * 1E-3)
stat.add("Velocity #sats", self.last_vel.n_sats)
if self.last_baseline:
stat.add("Baseline N", self.last_baseline.n * 1E-3)
stat.add("Baseline E", self.last_baseline.e * 1E-3)
stat.add("Baseline D", self.last_baseline.d * 1E-3)
stat.add("Baseline #sats", self.last_baseline.n_sats)
stat.add("Baseline fix mode", FIX_MODE[self.last_baseline.flags & 0x03])
if self.last_dops:
stat.add("GDOP", self.last_dops.gdop * 1E-2)
stat.add("PDOP", self.last_dops.pdop * 1E-2)
stat.add("TDOP", self.last_dops.tdop * 1E-2)
stat.add("HDOP", self.last_dops.hdop * 1E-2)
stat.add("VDOP", self.last_dops.vdop * 1E-2)
if not self.piksi:
stat.summary(DiagnosticStatus.ERROR, "Piksi not connected")
elif fix_mode < 0:
stat.summary(DiagnosticStatus.ERROR, "No fix")
elif fix_mode < 1:
stat.summary(DiagnosticStatus.WARN, "No RTK fix")
elif num_sats < 5:
stat.summary(DiagnosticStatus.WARN, "Low number of satellites in view")
else:
stat.summary(DiagnosticStatus.OK, "Piksi connected")
def spin(self):
reconnect_delay = 1.0
while not rospy.is_shutdown():
try:
rospy.loginfo("Connecting to SwiftNav Piksi on port %s" % self.piksi_port)
self.connect_piksi()
while not rospy.is_shutdown():
rospy.sleep(0.05)
if not self.piksi.is_alive():
raise IOError
self.diag_updater.update()
self.check_timeouts()
break # should only happen if rospy is trying to shut down
except IOError as e:
rospy.logerr("IOError")
self.disconnect_piksi()
except SystemExit as e:
rospy.logerr("Unable to connect to Piksi on port %s" % self.piksi_port)
self.disconnect_piksi()
except: # catch *all* exceptions
e = sys.exc_info()[0]
rospy.logerr("Uncaught error: %s" % repr(e))
self.disconnect_piksi()
rospy.loginfo("Attempting to reconnect in %fs" % reconnect_delay)
rospy.sleep(reconnect_delay)
