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)
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)
handler.add_callback(heartbeat_callback, msg_type=SBP_MSG_HEARTBEAT) # Generate publisher and callback function for Tracking messages if publish_tracking_state: pub_tracking_state = rospy.Publisher(rospy.get_name() + '/tracking_state', msg_tracking_state, queue_size = 10) tracking_state_msg = msg_tracking_state() handler.add_callback(tracking_state_callback, msg_type=SBP_MSG_TRACKING_STATE) # Generate publisher and callback function for Debug messages #init debug msg, required even tough publish_piksidebug is false to avoid run time errors debug_msg = PiksiDebug() debug_msg.num_sat = 0 #Unkown debug_msg.rtk_mode_fix = False #Unkown debug_msg.sat = [] #Unkown debug_msg.cn0 = [] #Unkown debug_msg.tracking_running = [] #Unkown debug_msg.system_error = 255 #Unkown debug_msg.io_error = 255 #Unkown debug_msg.swift_nap_error = 255 #Unkown debug_msg.external_antenna_present = 255 #Unkown if publish_piksidebug: pub_piksidebug = rospy.Publisher(rospy.get_name() + '/debug', PiksiDebug, queue_size = 10) handler.start() # Keep script alive while not rospy.is_shutdown(): rospy.sleep(0.5)
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)