From 57cbe07e0cfc89bd5d62c4cead0862b104a507be Mon Sep 17 00:00:00 2001 From: brad Date: Thu, 23 Jun 2016 11:22:33 -0400 Subject: [PATCH] change SPB_MSG_BASE_POS to SPB_MSG_BASE_POS_LLH --- src/piksi_driver.py | 203 +++++++++++++++++++++++++------------------- 1 file changed, 114 insertions(+), 89 deletions(-) diff --git a/src/piksi_driver.py b/src/piksi_driver.py index 2ce5622..6839269 100755 --- a/src/piksi_driver.py +++ b/src/piksi_driver.py @@ -5,25 +5,25 @@ # TODO add rinex logging (copy from https://github.com/swift-nav/piksi_tools/blob/master/piksi_tools/console/observation_view.py) # Import system libraries -import time -import sys +import ast import os import socket +import sys import threading +import time from collections import deque, defaultdict -import yaml -import ast +import yaml # Import ROS libraries import rospy -import roslib; roslib.load_manifest(PKG) +import roslib; + +roslib.load_manifest(PKG) import rospkg import diagnostic_updater -import diagnostic_msgs import tf2_ros from dynamic_reconfigure.server import Server, extract_params - # Import ROS messages from sensor_msgs.msg import NavSatFix, TimeReference, NavSatStatus from nav_msgs.msg import Odometry @@ -41,7 +41,7 @@ # Import SBP message ids from sbp.system import SBP_MSG_HEARTBEAT, SBP_MSG_STARTUP -from sbp.observation import SBP_MSG_OBS, SBP_MSG_BASE_POS, SBP_MSG_EPHEMERIS +from sbp.observation import SBP_MSG_OBS, SBP_MSG_BASE_POS_LLH, SBP_MSG_EPHEMERIS from sbp.navigation import SBP_MSG_GPS_TIME, SBP_MSG_BASELINE_NED, SBP_MSG_VEL_NED, SBP_MSG_POS_LLH, SBP_MSG_DOPS from sbp.logging import SBP_MSG_LOG from sbp.settings import * @@ -51,23 +51,26 @@ tree = lambda: defaultdict(tree) + def pretty(d, indent=0): - for key, value in d.iteritems(): - print '\t' * indent + str(key) - if isinstance(value, dict): - pretty(value, indent+1) - else: - print '\t' * (indent+1) + str(value) + for key, value in d.iteritems(): + print '\t' * indent + str(key) + if isinstance(value, dict): + pretty(value, indent + 1) + else: + print '\t' * (indent + 1) + str(value) + def extract_params_callback(config, parents=()): res = [] for name, g in config.iteritems(): - new_parents = parents+(g['name'],) + new_parents = parents + (g['name'],) for p in g['parameters']: res.append((new_parents, p, g['parameters'][p])) res.extend(extract_params_callback(g['groups'], parents=new_parents)) return res + """ About dynamic reconfigure for this driver. It turns out that dynamic reconfigure is one hell of a mess: @@ -81,8 +84,9 @@ def extract_params_callback(config, parents=()): We also name the piksi settings parameters as piksi__section__param in DR because there are multiple params in different sections called 'mode' for example. On startup, the node will load parameters from ~piksi/ and set those settings on the Piksi unit. It will also update the ~dynamic_reconfigure/ namespace where we store the parameters. """ -class GroupServer(Server): + +class GroupServer(Server): def __init__(self, type, callback, ns_prefix=(), nest_groups=True): self.ns_prefix = ns_prefix self.nest_groups = nest_groups @@ -90,18 +94,18 @@ def __init__(self, type, callback, ns_prefix=(), nest_groups=True): def _get_param_name(self, parents, name): if self.nest_groups: - return '~' + '/'.join(self.ns_prefix+parents+(name,)) + return '~' + '/'.join(self.ns_prefix + parents + (name,)) else: - return '~' + '/'.join(self.ns_prefix+(name,)) + return '~' + '/'.join(self.ns_prefix + (name,)) def _get_config_name(self, parents, name): - #return '__'.join(parents+(name,)) + # return '__'.join(parents+(name,)) return name def _extract_param_tree(self, desc, parents=()): res = [] for g in desc['groups']: - new_parents = parents+(g['name'],) + new_parents = parents + (g['name'],) for p in g['parameters']: res.append((new_parents, p)) res.extend(self._extract_param_tree(g, parents=new_parents)) @@ -111,13 +115,15 @@ def _copy_from_parameter_server(self): for parents, param in self._extract_param_tree(self.type.config_description): try: - self.config[self._get_config_name(parents, param['name'])] = rospy.get_param(self._get_param_name(parents, param['name'])) + self.config[self._get_config_name(parents, param['name'])] = rospy.get_param( + self._get_param_name(parents, param['name'])) except KeyError: pass def _copy_to_parameter_server(self): for parents, param in self._extract_param_tree(self.type.config_description): - rospy.set_param(self._get_param_name(parents, param['name']), self.config[self._get_config_name(parents,param['name'])]) + rospy.set_param(self._get_param_name(parents, param['name']), + self.config[self._get_config_name(parents, param['name'])]) def _clamp(self, config): @@ -130,17 +136,19 @@ def _clamp(self, config): elif val < minval and minval != "": config[param['name']] = minval + def calculate_utm_zone(lat, lon): """ Determine the UTM zone that a lon-lat point falls in. Returns and integer and a string, either ('N') or ('S'). """ if lat >= 0: - hemi = 'N' + hemi = 'N' else: - hemi = 'S' + hemi = 'S' zone = int((180 + lon) // 6) + 1 return (zone, hemi) + # Driver class for handling UDP connections for SBP class UDPDriver(BaseDriver): def __init__(self, host, port): @@ -148,7 +156,7 @@ def __init__(self, host, port): self.handle = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.handle.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) try: - #self.handle.connect((host, port)) + # self.handle.connect((host, port)) self.handle.bind(("", port)) except socket.error, msg: pass @@ -202,6 +210,7 @@ def __init__(self, host, port): def send(self, msg, **metadata): self.handle.sendto(msg.pack(), (self.address, self.port)) + class UDPReceiver(object): def __init__(self, host, port, callback): self._callback = callback @@ -210,9 +219,11 @@ def __init__(self, host, port, callback): self.piksi = Handler(self.framer) self.piksi.add_callback(self.callback) self.piksi.start() + def callback(self, msg, **metadata): self._callback(msg, **metadata) + class SerialSender(object): def __init__(self, port, baud_rate): import serial @@ -220,10 +231,10 @@ def __init__(self, port, baud_rate): self.serial.baudrate = baud self.serial.timeout = 1 - def send(self, msg, **metadata): self.serial.write(msg.pack()) + class SerialReceiver(object): def __init__(self, port, baud_rate, callback): self._callback = callback @@ -236,23 +247,27 @@ def __init__(self, port, baud_rate, callback): def callback(self, msg, **metadata): self._callback(msg, **metadata) + rospack = rospkg.RosPack() PKG_PATH = rospack.get_path(PKG) GPS_EPOCH = time.mktime((1980, 1, 6, 0, 0, 0, 0, 0, 0)) -WEEK_SECONDS = 60*60*24*7 +WEEK_SECONDS = 60 * 60 * 24 * 7 + def ros_time_from_sbp_time(msg): - return rospy.Time(GPS_EPOCH + msg.wn*WEEK_SECONDS + msg.tow * 1E-3 + msg.ns * 1E-9) + return rospy.Time(GPS_EPOCH + msg.wn * WEEK_SECONDS + msg.tow * 1E-3 + msg.ns * 1E-9) -def nested_dict_iter(d,res=[],parents=()): - for k,v in d.iteritems(): + +def nested_dict_iter(d, res=[], parents=()): + for k, v in d.iteritems(): if isinstance(v, dict): - nested_dict_iter(v, res, parents+(k,)) + nested_dict_iter(v, res, parents + (k,)) else: - res.append((parents+(k,), v)) + res.append((parents + (k,), v)) return res + # Maps piksi error levels to ROS logging functions PIKSI_LOG_LEVELS_TO_ROS = { 0: rospy.logfatal, @@ -274,6 +289,7 @@ def nested_dict_iter(d,res=[],parents=()): # Covariance for non-observed quantities COV_NOT_MEASURED = 1000.0 + class PiksiROS(object): def __init__(self): @@ -309,9 +325,9 @@ def __init__(self): 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.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 @@ -326,16 +342,18 @@ def __init__(self): 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) + 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.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.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) @@ -347,8 +365,8 @@ def read_piksi_settings_info(self): 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'] = 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 @@ -358,11 +376,11 @@ def read_piksi_settings_info(self): 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') + 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': + for p, v in config.iteritems(): + if p == 'groups': continue n = p.split('__') if self.piksi_settings[n[1]][n[2]] != v: @@ -373,7 +391,7 @@ def reconfig_callback(self, config, level): p_val = i['enum'][v] except: p_val = i['enum'][0] - self.piksi_set(n[1],n[2],p_val) + self.piksi_set(n[1], n[2], p_val) self.piksi_settings[n[1]][n[2]] = v return config @@ -408,7 +426,7 @@ def read_params(self): 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.rtk_v_accuracy = rospy.get_param("~rtk_v_accuracy", self.rtk_h_accuracy * 3) self.sbp_log = rospy.get_param('~sbp_log_file', None) @@ -418,7 +436,7 @@ def read_params(self): 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_update_settings = rospy.get_param('~piksi', {}) self.piksi_save_settings = rospy.get_param('~piksi_save_settings', False) def setup_comms(self): @@ -438,7 +456,8 @@ def setup_comms(self): 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)) + 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: @@ -451,17 +470,23 @@ def callback_external(self, msg, **metadata): 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) + 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_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) + 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() @@ -472,10 +497,10 @@ def setup_pubsub(self): 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.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) @@ -484,9 +509,9 @@ def piksi_start(self): 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: + # 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) + 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) @@ -504,9 +529,12 @@ def connect_piksi(self): 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_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) @@ -530,15 +558,15 @@ def set_serial_number(self, 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')) + # 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 + # 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): @@ -584,7 +612,6 @@ def callback_sbp_heartbeat(self, msg, **metadata): 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))) @@ -614,9 +641,9 @@ def callback_sbp_pos(self, msg, **metadata): 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 + 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 @@ -631,9 +658,9 @@ def callback_sbp_pos(self, msg, **metadata): # 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 + 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 @@ -655,13 +682,13 @@ def publish_odom(self): 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.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.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) @@ -677,9 +704,9 @@ def callback_sbp_baseline(self, msg, **metadata): 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.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 @@ -744,7 +771,6 @@ def callback_sbp_obs(self, msg, **metadata): 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 @@ -775,11 +801,10 @@ def callback_sbp_obs(self, msg, **metadata): 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))) + 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: @@ -790,7 +815,7 @@ def callback_sbp_base_pos(self, msg, **metadata): if not self.proj: self.init_proj((msg.lat, msg.lon)) - E,N = self.proj(msg.lon,msg.lat, inverse=False) + E, N = self.proj(msg.lon, msg.lat, inverse=False) self.transform.header.stamp = rospy.Time.now() self.transform.transform.translation.x = E @@ -801,7 +826,6 @@ def callback_sbp_base_pos(self, msg, **metadata): 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: @@ -810,11 +834,10 @@ def callback_sbp_settings_read_by_index_resp(self, msg, **metadata): except: pass - rospy.set_param('~piksi_original_settings/%s/%s' % (p[0],p[1]), p[2]) + 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)) @@ -902,23 +925,25 @@ def spin(self): self.diag_updater.update() self.check_timeouts() - break # should only happen if rospy is trying to shut down + 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 + 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) + def main(): node = PiksiROS() node.spin() + if __name__ == "__main__": - main() + main()