Merge pull request #63 from ethz-asl/feature/v2.1.14

Feature/v2.1.14

ethz_piksi_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ethz_piksi_ros</name>
-  <version>1.8.1</version>
+  <version>1.9.0</version>
   <description>Meta-package for the ethz_piksi_ros repository.</description>
 
   <maintainer email="marcot@ethz.ch">Marco Tranzatto</maintainer>

piksi_multi_rtk_ros/README.md

@@ -7,7 +7,7 @@ The piksi_multi_rtk_ros package has been tested under ROS Indigo and Ubuntu 14.0
 
 **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-Multi#settings).
-  
+
 ## Installation and Configuration
 **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).
@@ -22,8 +22,9 @@ 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 firmware version based on the current SBP Lib version.
 
-Currently the `install_piksi_multi.sh` will install **SBP Lib 2.3.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.5.12** 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.4.1** (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 2.1.14** from [SwiftNav Firmware page](https://support.swiftnav.com/customer/en/portal/articles/2492784-piksi-multi-and-duro-firmware) in your Piksi Multi.
+**WARNING: If upgrading from a firmware below v2.0.0 to a firmware greater than v2.0.0, you must upgrade to v2.0.0 first.**
 
 ## 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).
@@ -35,7 +36,7 @@ If you have already configured Piksi Multi to act as a base station and sampled
 roslaunch piksi_multi_rtk_ros piksi_multi_base_station.launch
 ```
 If you have already configured Piksi Multi to act as a base station (and did **NOT** sample its position), then launch the following file.
-Once the survey is over, the sampled position will be automatically written in Piksi flash memory. 
+Once the survey is over, the sampled position will be automatically written in Piksi flash memory.
 ```
 roslaunch piksi_multi_rtk_ros geodetic_survey.launch
 ```
@@ -66,7 +67,7 @@ The most interesting advertised topics are:
    this message contains ENU (East-North-Up) coordinate of the receiver in case of SPP fix. Orientation is set to identity quaternion (w=1);
  - `/piksi/enu_pose_best_fix` ([geometry_msgs/PointStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html))
    this message contains ENU (East-North-Up) coordinate of the receiver with best available fix at the moment (either RTK or SPP). Orientation is set to identity quaternion (w=1);
-   
+
 For a complete list of advertised topics please check function [`advertise_topics`](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/src/piksi_multi.py#L264).
 
 ### Raw IMU and Magnetometer Measurements
@@ -80,7 +81,7 @@ In the former case the following private parameters must be set:
  - `latitude0_deg`: latitude where the origin of ENU frame is located, in degrees;
  - `longitude0_deg`: longitude where the origin of ENU frame is located, in degrees;
  - `altitude0`: altitude where the origin of ENU frame is located, in meters;
- 
+
 see also configuration file [enu_origin_example.yaml](https://github.com/ethz-asl/ethz_piksi_ros/blob/master/piksi_multi_rtk_ros/cfg/enu_origin_example.yaml).
 
 ## Corrections Over Wifi

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.3.15 #version you want to checkout before installing
+REPO_TAG=v2.4.1 #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
@@ -40,6 +40,7 @@ fi
 cd ./python
 echo "Installing SBP dependencies."
 sudo apt-get install pandoc
+sudo apt-get install python-pip
 sudo pip install tox
 sudo pip install -r requirements.txt
 sudo pip install markupsafe

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.8.1</version>
+  <version>1.9.0</version>
   <description>
     ROS driver for Piksi Multi RTK GPS Receiver.
   </description>

piksi_multi_rtk_ros/src/piksi_multi_rtk_ros/piksi_multi.py

@@ -13,11 +13,11 @@
 import std_srvs.srv
 # Import message types
 from sensor_msgs.msg import NavSatFix, NavSatStatus
+import piksi_rtk_msgs # TODO(rikba): If we dont have this I get NameError: global name 'piksi_rtk_msgs' is not defined.
 from piksi_rtk_msgs.msg import (AgeOfCorrections, BaselineEcef, BaselineHeading, BaselineNed, BasePosEcef, BasePosLlh,
                                 DeviceMonitor_V2_3_15, DopsMulti, GpsTimeMulti, Heartbeat, ImuRawMulti,
-                                InfoWifiCorrections, Log, MagRaw, Observation, PosEcef, PosLlhMulti,
-                                ReceiverState_V2_3_15, TrackingState_V2_3_15,
-                                UartState_V2_3_15, UtcTimeMulti, VelEcef, VelNed)
+                                InfoWifiCorrections, Log, MagRaw, MeasurementState_V2_4_1, Observation, PosEcef, PosLlhMulti,
+                                ReceiverState_V2_4_1, UartState_V2_3_15, UtcTimeMulti, VelEcef, VelNed)
 from piksi_rtk_msgs.srv import *
 from geometry_msgs.msg import (PoseWithCovarianceStamped, PointStamped, PoseWithCovariance, Point, TransformStamped,
                                Transform)
@@ -51,7 +51,7 @@
 
 
 class PiksiMulti:
-    LIB_SBP_VERSION_MULTI = '2.3.15'  # SBP version used for Piksi Multi.
+    LIB_SBP_VERSION_MULTI = '2.4.1'  # SBP version used for Piksi Multi.
 
     # Geodetic Constants.
     kSemimajorAxis = 6378137
@@ -202,7 +202,7 @@ def create_topic_callbacks(self):
         self.handler.add_callback(self.cb_sbp_obs, msg_type=SBP_MSG_OBS)
         self.handler.add_callback(self.cb_sbp_settings_read_by_index_resp, msg_type=SBP_MSG_SETTINGS_READ_BY_INDEX_RESP)
         self.handler.add_callback(self.cb_settings_read_resp, msg_type=SBP_MSG_SETTINGS_READ_RESP)
-        self.handler.add_callback(self.cb_sbp_tracking_state, msg_type=SBP_MSG_TRACKING_STATE)
+        self.handler.add_callback(self.cb_sbp_measurement_state, msg_type=SBP_MSG_MEASUREMENT_STATE)
         self.handler.add_callback(self.cb_sbp_uart_state, msg_type=SBP_MSG_UART_STATE)
 
         # Callbacks generated "automatically".
@@ -272,7 +272,7 @@ def init_num_corrections_msg(self):
         return num_wifi_corrections
 
     def init_receiver_state_msg(self):
-        receiver_state_msg = ReceiverState_V2_3_15()
+        receiver_state_msg = ReceiverState_V2_4_1()
         receiver_state_msg.num_sat = 0  # Unknown.
         receiver_state_msg.rtk_mode_fix = False  # Unknown.
         receiver_state_msg.sat = []  # Unknown.
@@ -287,7 +287,7 @@ def init_receiver_state_msg(self):
         receiver_state_msg.cn0_sbas = []  # Unknown.
         receiver_state_msg.num_glonass_sat = 0  # Unknown.
         receiver_state_msg.cn0_glonass = []  # Unknown.
-        receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_UNKNOWN
+        receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_UNKNOWN
 
         return receiver_state_msg
 
@@ -306,10 +306,10 @@ def advertise_topics(self):
                                                  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_V2_3_15, queue_size=10)
+        publishers['measurement_state'] = rospy.Publisher(rospy.get_name() + '/measurement_state',
+                                                       piksi_rtk_msgs.msg.MeasurementState_V2_4_1, queue_size=10)
         publishers['receiver_state'] = rospy.Publisher(rospy.get_name() + '/debug/receiver_state',
-                                                       ReceiverState_V2_3_15, queue_size=10)
+                                                       ReceiverState_V2_4_1, 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)
@@ -663,21 +663,21 @@ def cb_sbp_pos_llh(self, msg_raw, **metadata):
         self.receiver_state_msg.rtk_mode_fix = True if (msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK) else False
 
         if msg.flags == PosLlhMulti.FIX_MODE_INVALID:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_INVALID
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_INVALID
         elif msg.flags == PosLlhMulti.FIX_MODE_SPP:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_SPP
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_SPP
         elif msg.flags == PosLlhMulti.FIX_MODE_DGNSS:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_DGNSS
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_DGNSS
         elif msg.flags == PosLlhMulti.FIX_MODE_FLOAT_RTK:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_FLOAT_RTK
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_FLOAT_RTK
         elif msg.flags == PosLlhMulti.FIX_MODE_FIX_RTK:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_FIXED_RTK
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_FIXED_RTK
         elif msg.flags == PosLlhMulti.FIX_MODE_DEAD_RECKONING:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.FIX_MODE_DEAD_RECKONING
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.FIX_MODE_DEAD_RECKONING
         elif msg.flags == PosLlhMulti.FIX_MODE_SBAS:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_SBAS
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_SBAS
         else:
-            self.receiver_state_msg.fix_mode = ReceiverState_V2_3_15.STR_FIX_MODE_UNKNOWN
+            self.receiver_state_msg.fix_mode = ReceiverState_V2_4_1.STR_FIX_MODE_UNKNOWN
 
         self.publish_receiver_state_msg()
 
@@ -779,21 +779,14 @@ def cb_sbp_heartbeat(self, msg_raw, **metadata):
         if self.base_station_mode:
             self.multicaster.sendSbpPacket(msg_raw)
 
-    def cb_sbp_tracking_state(self, msg_raw, **metadata):
-        msg = MsgTrackingState(msg_raw)
+    def cb_sbp_measurement_state(self, msg_raw, **metadata):
+        msg = MsgMeasurementState(msg_raw)
 
-        # print "\n\n++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"
-        #
-        # for single_tracking_state in msg.states:
-        #     print single_tracking_state
-        #     print "--------------------\n"
-
-        tracking_state_msg = TrackingState_V2_3_15()
-        tracking_state_msg.header.stamp = rospy.Time.now()
-        tracking_state_msg.sat = []
-        tracking_state_msg.code = []
-        tracking_state_msg.fcn = []
-        tracking_state_msg.cn0 = []
+        measurement_state_msg = piksi_rtk_msgs.msg.MeasurementState_V2_4_1()
+        measurement_state_msg.header.stamp = rospy.Time.now()
+        measurement_state_msg.sat = []
+        measurement_state_msg.code = []
+        measurement_state_msg.cn0 = []
 
         # Temporary variables for receiver state message.
         num_gps_sat = 0
@@ -802,52 +795,71 @@ def cb_sbp_tracking_state(self, msg_raw, **metadata):
         cn0_sbas = []
         num_glonass_sat = 0
         cn0_glonass = []
+        num_bds_sat = 0
+        cn0_bds = []
+        num_gal_sat = 0
+        cn0_gal = []
 
-        for single_tracking_state in msg.states:
+        for single_measurement_state in msg.states:
 
             # Use satellites with valid cn0.
-            if single_tracking_state.cn0 > 0.0:
+            if single_measurement_state.cn0 > 0.0:
 
-                tracking_state_msg.sat.append(single_tracking_state.sid.sat)
-                tracking_state_msg.code.append(single_tracking_state.sid.code)
-                tracking_state_msg.fcn.append(single_tracking_state.fcn)
-                tracking_state_msg.cn0.append(single_tracking_state.cn0)
+                measurement_state_msg.sat.append(single_measurement_state.mesid.sat)
+                measurement_state_msg.code.append(single_measurement_state.mesid.code)
+                measurement_state_msg.cn0.append(single_measurement_state.cn0)
 
                 # Receiver state fields.
-                code = single_tracking_state.sid.code
-                if code == TrackingState_V2_3_15.CODE_GPS_L1CA or \
-                                code == TrackingState_V2_3_15.CODE_GPS_L2CM or \
-                                code == TrackingState_V2_3_15.CODE_GPS_L1P or \
-                                code == TrackingState_V2_3_15.CODE_GPS_L2P:
+                code = single_measurement_state.mesid.code
+                if code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L1CA or \
+                   code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L2CM or \
+                   code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L1P or \
+                   code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GPS_L2P:
                     num_gps_sat += 1
-                    cn0_gps.append(single_tracking_state.cn0)
+                    cn0_gps.append(single_measurement_state.cn0)
 
-                if code == TrackingState_V2_3_15.CODE_SBAS_L1CA:
+                elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_SBAS_L1CA:
                     num_sbas_sat += 1
-                    cn0_sbas.append(single_tracking_state.cn0)
+                    cn0_sbas.append(single_measurement_state.cn0)
 
-                if code == TrackingState_V2_3_15.CODE_GLO_L1CA or \
-                                code == TrackingState_V2_3_15.CODE_GLO_L1CA:
+                elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GLO_L1CA or \
+                     code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GLO_L2CA:
                     num_glonass_sat += 1
-                    cn0_glonass.append(single_tracking_state.cn0)
+                    cn0_glonass.append(single_measurement_state.cn0)
+
+                elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_BDS2_B1 or \
+                     code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_BDS2_B2:
+                    num_bds_sat += 1
+                    cn0_bds.append(single_measurement_state.cn0)
+
+                elif code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GAL_E1B or \
+                     code == piksi_rtk_msgs.msg.MeasurementState_V2_4_1.CODE_GAL_E7I:
+                    num_gal_sat += 1
+                    cn0_gal.append(single_measurement_state.cn0)
+
+                else:
+                    rospy.logwarn("[cb_sbp_measurement_state]: Unknown satellite code %d.", code)
 
         # Publish if there's at least one element in each array.
-        if len(tracking_state_msg.sat) \
-                and len(tracking_state_msg.code) \
-                and len(tracking_state_msg.fcn) \
-                and len(tracking_state_msg.cn0):
-            self.publishers['tracking_state'].publish(tracking_state_msg)
+        if len(measurement_state_msg.sat) \
+                and len(measurement_state_msg.code) \
+                and len(measurement_state_msg.cn0):
+            self.publishers['measurement_state'].publish(measurement_state_msg)
 
             # Update debug msg and publish.
-            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_sat = num_gps_sat + num_sbas_sat + num_glonass_sat + num_bds_sat + num_gal_sat
+            self.receiver_state_msg.sat = measurement_state_msg.sat
+            self.receiver_state_msg.cn0 = measurement_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
             self.receiver_state_msg.cn0_sbas = cn0_sbas
             self.receiver_state_msg.num_glonass_sat = num_glonass_sat
             self.receiver_state_msg.cn0_glonass = cn0_glonass
+            self.receiver_state_msg.num_bds_sat = num_bds_sat
+            self.receiver_state_msg.cn0_bds = cn0_bds
+            self.receiver_state_msg.num_gal_sat = num_gal_sat
+            self.receiver_state_msg.cn0_gal = cn0_gal
 
             self.publish_receiver_state_msg()
 

piksi_rtk_kml/package.xml

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

piksi_rtk_msgs/CMakeLists.txt

@@ -35,13 +35,15 @@ add_message_files(
     InfoWifiCorrections.msg
     Log.msg
     MagRaw.msg
+    MeasurementState_V2_4_1.msg
     Observation.msg
     PosEcef.msg
     PosLlh.msg
     PosLlhMulti.msg
     ReceiverState.msg
     ReceiverState_V2_2_15.msg
     ReceiverState_V2_3_15.msg
+    ReceiverState_V2_4_1.msg
     TrackingState.msg
     TrackingState_V2_2_15.msg
     TrackingState_V2_3_15.msg

piksi_rtk_msgs/msg/MeasurementState_V2_4_1.msg

@@ -0,0 +1,23 @@
+# The tracking message returns a variable-length array of tracking channel states. It reports status and
+# carrier-to-noise density measurements for all tracked satellites.
+
+# Message definition based on libsbp v2.4.1
+
+Header header
+
+uint8[] sat     # Constellation-specific satellite identifier.
+uint8[] code    # Signal constellation, band and code.
+uint8[] cn0     # Carrier-to-Noise density. Zero implies invalid cn0 [dB Hz / 4].
+
+
+uint8 CODE_GPS_L1CA     = 0
+uint8 CODE_GPS_L2CM     = 1
+uint8 CODE_SBAS_L1CA    = 2
+uint8 CODE_GLO_L1CA     = 3
+uint8 CODE_GLO_L2CA     = 4
+uint8 CODE_GPS_L1P      = 5
+uint8 CODE_GPS_L2P      = 6
+uint8 CODE_BDS2_B1      = 12
+uint8 CODE_BDS2_B2      = 13
+uint8 CODE_GAL_E1B      = 14
+uint8 CODE_GAL_E7I      = 20