Merge pull request #16 from ethz-asl/feature/lib_sbp_2_3_10

LibSBP 2.3.10

.gitignore

@@ -6,7 +6,6 @@ src/*.pyc
 geodetic_survey_bags/*.bag
 
 *.pyc
-init_rovio_npose0/launch/debug_tf_frames.launch
 
 #QT files
 *.files
@@ -16,7 +15,7 @@ init_rovio_npose0/launch/debug_tf_frames.launch
 *.config
 
 # Logs geodetic surveys
-piksi_multi_rtk_ros/log_surveys/2*.txt
+/piksi_multi_rtk_ros/log_surveys/
 
 # KML files.
 /kml/*

ethz_piksi_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ethz_piksi_ros</name>
-  <version>1.0.6</version>
+  <version>1.1.0</version>
   <description>Meta-package for the ethz_piksi_ros repository.</description>
 
   <maintainer email="[email protected]">Marco Tranzatto</maintainer>

piksi_multi_rtk_ros/README.md

@@ -2,27 +2,28 @@ piksi_multi_rtk_gps
 ======
 ROS node to read SBP messages from an attached Piksi **Multi** RTK device.
 
-The piksi_multi_rtk_ros package has been tested under:
-  * [ROS] Indigo and Ubuntu 14.04, and [ROS] Kinetic and Ubuntu 16.04;
-  * The latest version relies on Piksi Multi Firmware **1.2.14** release (see [Swift Nav firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware)).
+The piksi_multi_rtk_ros package has been tested under ROS Indigo and Ubuntu 14.04, and ROS Kinetic and Ubuntu 16.04.
 
-**WARNING:** default baud rate of the driver is set to '230400' (default baud rate of Piksi Multi is '115200'). This means you have to set your Piksi Multi baud rate correctly by following [these settings instructions](https://github.com/ethz-asl/ethz_piksi_ros/wiki/Installing-and-Configuring-Your-Piksi#settings).
+
+**WARNING:** default baud rate of the driver is set to '230400' (default baud rate of Piksi Multi is '115200').
+This means you have to set your Piksi Multi baud rate correctly by following [these settings instructions](https://github.com/ethz-asl/ethz_piksi_ros/wiki/Installing-and-Configuring-Your-Piksi#settings).
 
 ## Installation and Configuration
-**WARNING: install __ONLY ONE__ version of SBP library, depending of which Hardware version you are using. This page cointains the driver for [Piksi Multi](https://www.swiftnav.com/piksi-multi).
+**WARNING: install __ONLY ONE__ version of SBP library, depending of which Hardware version you are using. This page contains the driver for [Piksi Multi](https://www.swiftnav.com/piksi-multi).
 If you are using [Piksi V2](http://docs.swiftnav.com/pdfs/piksi_datasheet_v2.3.1.pdf) please check its driver version: [piksi_rtk_gps](https://github.com/ethz-asl/mav_rtk_gps/tree/master/piksi_rtk_gps)** (it is not supported anymore).
 
-The following code will automatically download the required version of libsbp and install it in the default folder `/usr/local/lib/python2.7/dist-packages/sbp-2.2.15-py2.7.egg/sbp/`.
+The following code will automatically download the required version of libsbp and install it in the default folder `/usr/local/lib/python2.7/dist-packages/sbp-<SBP_LIB_VERSION>-py2.7.egg/sbp/`.
 
 ```
 # From the repository folder
 source install/install_piksi_multi.sh
 ```
 
 ### Firmware and SBP Lib Version
-Please check [here](https://support.swiftnav.com/customer/en/portal/articles/2492810-swift-binary-protocol) which Piksi Multi fimrware version based on the current SBP Lib version.
+Please check [here](https://support.swiftnav.com/customer/en/portal/articles/2492810-swift-binary-protocol) which Piksi Multi firmware version based on the current SBP Lib version.
 
-Currently the `install_piksi_multi.sh` will install SBP Lib 2.2.15 (see [REPO_TAG](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/install/install_piksi_multi.sh#L4)). This means you are supposed to install Firmware 1.2.14 from [SwiftNav Firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware) in your Piksi Multi.
+Currently the `install_piksi_multi.sh` will install **SBP Lib 2.3.10** (see [REPO_TAG](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/install/install_piksi_multi.sh#L4)).
+This means you are supposed to install **Firmware 1.4.10** from [SwiftNav Firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware) in your Piksi Multi.
 
 ## Usage
 **Make sure** you configured your Piksi(s) by following [these instructions](https://github.com/ethz-asl/ethz_piksi_ros/wiki/Installing-and-Configuring-Your-Piksi).

piksi_multi_rtk_ros/install/install_piksi_multi.sh

@@ -1,7 +1,7 @@
 #!/bin/bash
 
 GIT_REPO_LIBSBP=https://github.com/swift-nav/libsbp.git
-REPO_TAG=v2.2.15 #version you want to checkout before installing
+REPO_TAG=v2.3.10 #version you want to checkout before installing
 
 # Sort of reg express used to see if there is another verision on SBP library already installed
 # Adapted from https://stackoverflow.com/questions/6363441/check-if-a-file-exists-with-wildcard-in-shell-script

piksi_multi_rtk_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>piksi_multi_rtk_ros</name>
-  <version>1.0.6</version>
+  <version>1.1.0</version>
   <description>
     ROS driver for Piksi Multi RTK GPS Receiver.
   </description>

piksi_multi_rtk_ros/rosbag_record_scripts/debug_all.sh

@@ -0,0 +1,21 @@
+#! /bin/bash
+# record topics necessary to debug Piksi Multi. 
+# WARNING: remember to set rosparam "debug_mode" to true before launching the ROS driver!
+INITIAL_PATH=$(pwd)
+NOW="$(date +"%F-%H-%M-%S")"
+FOLDER_NAME="$(date +"%Y-%m-%d")"
+
+mkdir -p ~/bags/$FOLDER_NAME
+cd ~/bags/$FOLDER_NAME
+
+echo " ========================= "
+echo "Record topics necessary to debug Piksi Multi. Rosbag saved in '~/bags/<DATE_OF_TODAY>/'"
+echo "WARNING: remember to set rosparam 'debug_mode' to true before launching the ROS driver!"
+echo " ========================= "
+
+rosparam dump piksi-multi-debug-"$NOW".yaml
+
+rosbag record --output-name=piksi-multi-debug-"$NOW".bag \
+-e "/piksi/(.*)" \
+
+cd $INITIAL_PATH

piksi_multi_rtk_ros/src/piksi_multi.py

@@ -26,8 +26,8 @@
 from sbp.system import *
 from sbp.tracking import *  # WARNING: tracking is part of the draft messages, could be removed in future releases of libsbp.
 from sbp.piksi import *  # WARNING: piksi is part of the draft messages, could be removed in future releases of libsbp.
-from sbp.observation import SBP_MSG_OBS, SBP_MSG_OBS_DEP_A, SBP_MSG_OBS_DEP_B, SBP_MSG_BASE_POS_LLH, \
-    SBP_MSG_BASE_POS_ECEF
+from sbp.observation import *
+from sbp.orientation import * # WARNING: orientation messages are still draft messages.
 from sbp.piksi import MsgUartState, SBP_MSG_UART_STATE
 from sbp.settings import *
 from zope.interface.exceptions import Invalid
@@ -46,7 +46,7 @@
 
 
 class PiksiMulti:
-    LIB_SBP_VERSION_MULTI = '2.2.15'  # SBP version used for Piksi Multi.
+    LIB_SBP_VERSION_MULTI = '2.3.10'  # SBP version used for Piksi Multi.
 
     # Geodetic Constants.
     kSemimajorAxis = 6378137
@@ -75,7 +75,7 @@ def __init__(self):
                           "Please run the install script: 'install/install_piksi_multi.sh'" % (
                               installed_sbp_version, PiksiMulti.LIB_SBP_VERSION_MULTI))
 
-        # Open a connection to Piksi.
+        # Open a connection to SwiftNav receiver.
         interface = rospy.get_param('~interface', 'serial')
 
         if interface == 'tcp':
@@ -105,8 +105,8 @@ def __init__(self):
             rospy.loginfo("Swift driver started in debug mode, every available topic will be published.")
             # Debugging parameters.
             debug_delayed_corrections_stack_size = rospy.get_param('~debug_delayed_corrections_stack_size', 10)
-            self.received_corrections_fifo_stack = collections.deque([], debug_delayed_corrections_stack_size)
-            rospy.loginfo("Debug mode: delayed corrections stack size: %d." % debug_delayed_corrections_stack_size)
+            #self.received_corrections_fifo_stack = collections.deque([], debug_delayed_corrections_stack_size)
+            #rospy.loginfo("Debug mode: delayed corrections stack size: %d." % debug_delayed_corrections_stack_size)
         else:
             rospy.loginfo("Swift driver started in normal mode.")
 
@@ -116,7 +116,7 @@ def __init__(self):
         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')
+            '192.168.0.1')
         self.multicaster = []
         self.multicast_recv = []
 
@@ -191,6 +191,8 @@ def create_topic_callbacks(self):
         self.handler.add_callback(self.uart_state_callback, msg_type=SBP_MSG_UART_STATE)
         self.handler.add_callback(self.settings_read_resp, msg_type=SBP_MSG_SETTINGS_READ_RESP)
         self.handler.add_callback(self.settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
+        self.handler.add_callback(self.callback_sbp_obs, msg_type=SBP_MSG_OBS)
+        self.handler.add_callback(self.callback_sbp_base_pos_ecef, msg_type=SBP_MSG_BASE_POS_ECEF)
 
         # Callbacks generated "automatically".
         self.init_callback('baseline_ecef_multi', BaselineEcef,
@@ -227,22 +229,21 @@ def create_topic_callbacks(self):
         self.init_callback('age_of_corrections', AgeOfCorrections,
                            SBP_MSG_AGE_CORRECTIONS, MsgAgeCorrections, 'tow', 'age')
 
+        # Only if debug mode
+        if self.debug_mode:
+            self.handler.add_callback(self.callback_sbp_base_pos_llh, msg_type=SBP_MSG_BASE_POS_LLH)
+
         # 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.
+        # Relay "corrections" messages via UDP if in base station mode.
         if self.base_station_mode:
-            rospy.loginfo("Starting in base station mode")
+            rospy.loginfo("Starting device 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")
+            rospy.loginfo("Starting device in Rover Mode")
             self.multicast_recv = UdpHelpers.SbpUdpMulticastReceiver(self.udp_port, self.multicast_callback)
 
     def init_num_corrections_msg(self):
@@ -348,7 +349,12 @@ def advertise_topics(self):
                                                        DopsMulti, queue_size=10)
             publishers['pos_ecef_multi'] = rospy.Publisher(rospy.get_name() + '/pos_ecef',
                                                            PosEcef, queue_size=10)
-
+            publishers['observation'] = rospy.Publisher(rospy.get_name() + '/observation',
+                                                        Observation, queue_size=10)
+            publishers['base_pos_llh'] = rospy.Publisher(rospy.get_name() + '/base_pos_llh',
+                                                         BasePosLlh, queue_size=10)
+            publishers['base_pos_ecef'] = rospy.Publisher(rospy.get_name() + '/base_pos_ecef',
+                                                          BasePosEcef, queue_size=10)
         if not self.base_station_mode:
             publishers['wifi_corrections'] = rospy.Publisher(rospy.get_name() + '/debug/wifi_corrections',
                                                              InfoWifiCorrections, queue_size=10)
@@ -466,40 +472,91 @@ def init_callback(self, topic_name, ros_datatype, sbp_msg_type, callback_data_ty
             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(self, msg_raw, **metadata):
+        if self.debug_mode:
+            msg = MsgObs(msg_raw)
+
+            obs_msg = Observation()
+            obs_msg.header.stamp = rospy.Time.now()
+
+            obs_msg.tow = msg.header.t.tow
+            obs_msg.ns_residual = msg.header.t.ns_residual
+            obs_msg.wn = msg.header.t.wn
+            obs_msg.n_obs = msg.header.n_obs
+
+            obs_msg.P = []
+            obs_msg.L_i = []
+            obs_msg.L_f = []
+            obs_msg.D_i = []
+            obs_msg.D_f = []
+            obs_msg.cn0 = []
+            obs_msg.lock = []
+            obs_msg.flags = []
+            obs_msg.sid_sat = []
+            obs_msg.sid_code = []
+
+            for observation in msg.obs:
+                obs_msg.P.append(observation.P)
+                obs_msg.L_i.append(observation.L.i)
+                obs_msg.L_f.append(observation.L.f)
+                obs_msg.D_i.append(observation.D.i)
+                obs_msg.D_f.append(observation.D.f)
+                obs_msg.cn0.append(observation.cn0)
+                obs_msg.lock.append(observation.lock)
+                obs_msg.flags.append(observation.flags)
+                obs_msg.sid_sat.append(observation.sid.sat)
+                obs_msg.sid_code.append(observation.sid.code)
+
+            self.publishers['observation'].publish(obs_msg)
+
+        if self.base_station_mode:
+            self.multicaster.sendSbpPacket(msg_raw)
+
+    def callback_sbp_base_pos_llh(self, msg_raw, **metadata):
+        if self.debug_mode:
+            msg = MsgBasePosLLH(msg_raw)
+
+            pose_llh_msg = BasePosLlh()
+            pose_llh_msg.header.stamp = rospy.Time.now()
+
+            pose_llh_msg.lat = msg.lat
+            pose_llh_msg.lon = msg.lon
+            pose_llh_msg.height = msg.height
 
-    def callback_sbp_obs_dep_a(self, msg, **metadata):
-        # rospy.logwarn("CALLBACK SBP OBS DEP A")
-        self.multicaster.sendSbpPacket(msg)
+            self.publishers['base_pos_llh'].publish(pose_llh_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_ecef(self, msg_raw, **metadata):
+        if self.debug_mode:
+            msg = MsgBasePosECEF(msg_raw)
 
-    def callback_sbp_base_pos_llh(self, msg, **metadata):
-        # rospy.logwarn("CALLBACK SBP OBS BASE LLH")
-        self.multicaster.sendSbpPacket(msg)
+            pose_ecef_msg = BasePosEcef()
+            pose_ecef_msg.header.stamp = rospy.Time.now()
 
-    def callback_sbp_base_pos_ecef(self, msg, **metadata):
-        # rospy.logwarn("CALLBACK SBP OBS BASE LLH")
-        self.multicaster.sendSbpPacket(msg)
+            pose_ecef_msg.x = msg.x
+            pose_ecef_msg.y = msg.y
+            pose_ecef_msg.z = msg.z
+
+            self.publishers['base_pos_ecef'].publish(pose_ecef_msg)
+
+        if self.base_station_mode:
+            self.multicaster.sendSbpPacket(msg_raw)
 
     def multicast_callback(self, msg, **metadata):
         # rospy.logwarn("MULTICAST Callback")
         if self.framer:
 
-            if self.debug_mode:
-                # Test network delay by storing a fixed number of correction messages and retrieving the oldest one.
-                # TODO (marco-tranzatto) check if we need to store even **metadata or not
-                # self.received_corrections_fifo_stack.append([msg, **metadata])
-                # oldest_correction = self.received_corrections_fifo_stack.popleft()
-                self.received_corrections_fifo_stack.append(msg)
-                oldest_correction = self.received_corrections_fifo_stack.popleft()
-                self.framer(oldest_correction, **metadata)
-            else:
-                self.framer(msg, **metadata)
+            # TODO (marco-tranzatto) probably next commented part should be completely deleted.
+            # if self.debug_mode:
+            #     # Test network delay by storing a fixed number of correction messages and retrieving the oldest one.
+            #     # TODO (marco-tranzatto) check if we need to store even **metadata or not
+            #     # self.received_corrections_fifo_stack.append([msg, **metadata])
+            #     # oldest_correction = self.received_corrections_fifo_stack.popleft()
+            #     self.received_corrections_fifo_stack.append(msg)
+            #     oldest_correction = self.received_corrections_fifo_stack.popleft()
+            #     self.framer(oldest_correction, **metadata)
+            # else:
+            #     self.framer(msg, **metadata)
+            self.framer(msg, **metadata)
 
             # Publish debug message about wifi corrections, if enabled.
             self.num_wifi_corrections.header.seq += 1
@@ -714,15 +771,9 @@ def tracking_state_callback(self, msg_raw, **metadata):
             self.publishers['tracking_state'].publish(tracking_state_msg)
 
             # Update debug msg and publish.
-            self.receiver_state_msg.num_sat = 0
-            for satellite_cno in tracking_state_msg.cn0:
-                # Use number of satellites with valid cn0.
-                if satellite_cno > 0.0:
-                    self.receiver_state_msg.num_sat += 1
-
+            self.receiver_state_msg.num_sat = num_gps_sat + num_sbas_sat + num_glonass_sat
             self.receiver_state_msg.sat = tracking_state_msg.sat
             self.receiver_state_msg.cn0 = tracking_state_msg.cn0
-
             self.receiver_state_msg.num_gps_sat = num_gps_sat
             self.receiver_state_msg.cn0_gps = cn0_gps
             self.receiver_state_msg.num_sbas_sat = num_sbas_sat

piksi_rtk_kml/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>piksi_rtk_kml</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>
     ROS node to write KML file from Piksi messages.
   </description>