Merge pull request #39 from ethz-asl/feature/firmware_1_5_12

Feature/firmware 1.5.12 with lib SBP 2.3.15

ethz_piksi_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ethz_piksi_ros</name>
-  <version>1.4.0</version>
+  <version>1.5.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

@@ -15,15 +15,15 @@ If you are using [Piksi V2](http://docs.swiftnav.com/pdfs/piksi_datasheet_v2.3.1
 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
+# Execute this line in the package folder 'piksi_multi_rtk_ros'
 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.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.
+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.
 
 ## 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).
@@ -69,6 +69,10 @@ The most interesting advertised topics are:
 
 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
+Raw IMU and magnetometer measurements are not published by default. If you want to publish them in ROS you need to: (a) Open Swifnav console, connect to Piksi, navigate to "Settings", and then in the "imu" section enable "imu raw output" and "mag raw output" (save these new settings, so you need to do this operation only once); (b) set to true the flag [publish_raw_imu_and_mag](./cfg/piksi_multi_driver_settings.yaml#L19).
+Raw measurements are published in `/piksi/imu_raw` and `/piksi/mag_raw`.
+
 ## Origin ENU Frame
 The origin of the ENU (East-North-Up) frame is set either using rosparameters or using the first RTK fix message obtained.
 In the former case the following private parameters must be set:

piksi_multi_rtk_ros/cfg/piksi_multi_driver_settings.yaml

@@ -2,24 +2,28 @@
 
 # interface: specifies the interface over which to connect.
 # supported values are 'serial' and 'tcp'
-# if this value is not valid, the driver will default to serial communication
+# if this value is not valid, the driver will default to serial communication.
 interface: 'serial'
 
 # set this to true if the Swift device is acting as RTK Base Station.
 base_station_mode: false
 
-# if true, output every topic read by this driver
+# if true, output every topic read by this driver.
 debug_mode: false
 
 # if true the driver is verbose.
 driver_verbose: true
 
+# if true, raw imu and magnetometer are published
+# (messages are available only if enabled with the swiftnav console).
+publish_raw_imu_and_mag: false
+
 # broadcast_addr: ip address to use for broadcasting corrections
 # whenever possible, use network broadcast, not global ip broadcast
 # use ifconfig to show network broadcast (or calculate it yourself....).
 broadcast_addr: 192.168.0.255
 
-# broadcast_port: port to use for broadcasting corrections
+# broadcast_port: port to use for broadcasting corrections.
 broadcast_port: 26078
 
 # base_station_ip_for_latency_estimation: IP of base station for
@@ -33,6 +37,8 @@ navsatfix_frame_id: 'base_link'
 # Variance to be published in the NavSatFix message (in diagonal part).
 # Single Point Positioning (SPP).
 var_spp: [25.0, 25.0, 64.0]
+# Single Point Positioning (SPP) with SBAS.
+var_spp_sbas: [1.0, 1.0, 1.0]
 # RTK float mode.
 var_rtk_float: [25.0, 25.0, 64.0]
 # RTK fix mode.
@@ -41,7 +47,7 @@ var_rtk_fix: [0.0049, 0.0049, 0.01]
 # Serial Settings
 
 # serial_port: defines which serial device to use for communication
-# ex '/dev/ttyUSB0'
+# ex '/dev/ttyUSB0'.
 serial_port: '/dev/ttyUSB0'
 
 # baud_rate: defines the rate for the serial connection. This driver uses

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.10 #version you want to checkout before installing
+REPO_TAG=v2.3.15 #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/launch/geodetic_survey.launch

@@ -4,7 +4,11 @@
 
   <!-- Parameters. -->
   <arg name="number_of_desired_fixes"                 default="5000"/>
-  <arg name="height_base_station_from_ground"         default="0.0"/>
+  <!-- Height antenna base station from ground for RSL-ETHZ tripod.
+       This parameter is used only for convenience by RSL-ETHZ staff to
+       create the origin of the ENU frame on the ground below the antenna of the base station.
+       The surveyed position of the antenna is NOT affected by this parameter. -->
+  <arg name="height_base_station_from_ground"         default="1.89"/>
 
   <!-- Piksi. -->
   <include file="$(find piksi_multi_rtk_ros)/launch/piksi_multi_base_station.launch"/>

piksi_multi_rtk_ros/launch/piksi_multi_base_station.launch

@@ -2,13 +2,28 @@
 
 <launch>
 
-  <node pkg="piksi_multi_rtk_ros" type="piksi_multi.py" name="piksi" output="screen" respawn="true">
+  <!-- Settings: will overwrite default setting loaded from file.  -->
+  <!-- Available interfaces where Piksi Multi can be connected: serial or tcp. -->
+  <arg name="interface"                   default="serial"/>
+  <!-- If interface is serial, this specifies baud rate of Piksi Multi. -->
+  <arg name="baud_rate"                   default="230400"/>
+  <!-- If interface is tcp, this specifies the address of Piksi Multi. -->
+  <arg name="tcp_addr"                    default="192.168.0.222"/>
+  <!-- Broadcast address for corrections: used to send corrections over WiFi. -->
+  <arg name="broadcast_addr"              default="192.168.0.255"/>
+
+  <!-- Piksi Multi ROS driver node: base station mode -->
+  <node pkg="piksi_multi_rtk_ros" type="piksi_multi.py" name="piksi_multi_base_station" output="screen" respawn="true">
     <!-- Load default settings -->
     <rosparam file="$(find piksi_multi_rtk_ros)/cfg/piksi_multi_driver_settings.yaml"/>
 
-    <!-- Overwrtie needed settings -->
-    <param name="base_station_mode" value="true"/>
-    <param name="broadcast_addr" value="192.168.0.255"/> <!--'ifconfig' to check Bcast -->
+    <!-- Overwrite settings -->
+    <param name="interface"               value="$(arg interface)"/>
+    <param name="baud_rate"               value="$(arg baud_rate)"/>
+    <param name="tcp_addr"                value="$(arg tcp_addr)"/>
+    <param name="broadcast_addr"          value="$(arg broadcast_addr)"/>
+    <!-- Force base station mode -->
+    <param name="base_station_mode"       value="true"/>
   </node>
 
 </launch>

piksi_multi_rtk_ros/launch/piksi_multi_rover.launch

@@ -1,24 +1,35 @@
 <?xml version="1.0"?>
 
 <launch>
-  <!-- Node name -->
-  <arg name="node_name"                   value="piksi"/>
-
-  <!-- Arguments -->
+  <!-- Settings: will overwrite default setting loaded from file.  -->
+  <!-- Available interfaces where Piksi Multi can be connected: serial or tcp. -->
+  <arg name="interface"                   default="serial"/>
+  <!-- If interface is serial, this specifies baud rate of Piksi Multi. -->
+  <arg name="baud_rate"                   default="230400"/>
+  <!-- If interface is tcp, this specifies the address of Piksi Multi. -->
+  <arg name="tcp_addr"                    default="192.168.0.222"/>
+  <!-- IP of a "pingable" device, used to check network status. -->
   <arg name="base_station_ip"             default="192.168.0.1"/>
+  <!-- Enable/disable load of an origin position of ENU frame. -->
   <arg name="load_enu_origin_from_file"   default="false"/>
+  <!-- Location of the file containing origin ENU frame, used if 'load_enu_origin_from_file' is true. -->
   <arg name="enu_origin_file"             default="$(find piksi_multi_rtk_ros)/cfg/enu_origin_example.yaml"/>
 
-  <node pkg="piksi_multi_rtk_ros" type="piksi_multi.py" name="$(arg node_name)" output="screen" respawn="true">
+  <!-- Piksi Multi ROS driver node: rover mode -->
+  <node pkg="piksi_multi_rtk_ros" type="piksi_multi.py" name="piksi" output="screen" respawn="true">
     <!-- Load default settings -->
     <rosparam file="$(find piksi_multi_rtk_ros)/cfg/piksi_multi_driver_settings.yaml"/>
 
     <!-- Load enu origin from file -->
     <rosparam file="$(arg enu_origin_file)"   if="$(arg load_enu_origin_from_file)"/>
 
     <!-- Overwrite settings -->
-    <param name="base_station_mode"                       value="false"/>
+    <param name="interface"                               value="$(arg interface)"/>
+    <param name="baud_rate"                               value="$(arg baud_rate)"/>
+    <param name="tcp_addr"                                value="$(arg tcp_addr)"/>
     <param name="base_station_ip_for_latency_estimation"  value="$(arg base_station_ip)"/>
+    <!-- Force rover mode -->
+    <param name="base_station_mode"                       value="false"/>
   </node>
 
 </launch>

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

piksi_multi_rtk_ros/src/geodetic_survey.py

@@ -8,7 +8,7 @@
 import rospy
 from piksi_rtk_msgs.srv import *
 import std_srvs.srv
-from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import (NavSatFix, NavSatStatus)
 import os
 import time
 
@@ -29,22 +29,29 @@ def __init__(self):
 
         # Settings
         self.number_of_desired_fixes = rospy.get_param('~number_of_desired_fixes', 5000)
-        self.spp_topics_name = rospy.get_param('~spp_topics_name', 'piksi/navsatfix_spp')
-        self.write_settings_service_name = rospy.get_param('~write_settings_service_name', 'piksi/settings_write')
-        self.save_settings_service_name = rospy.get_param('~save_settings_service_name', 'piksi/settings_save')
+        self.navsatfix_topics_name = rospy.get_param('~navsatfix_topics_name', 'piksi_multi_base_station/navsatfix_spp')
+        self.write_settings_service_name = rospy.get_param('~write_settings_service_name', 'piksi_multi_base_station/settings_write')
+        self.save_settings_service_name = rospy.get_param('~save_settings_service_name', 'piksi_multi_base_station/settings_save')
         self.read_req_settings_service_name = rospy.get_param('~read_req_settings_service_name',
-                                                              'piksi/settings_read_req')
+                                                              'piksi_multi_base_station/settings_read_req')
         self.read_resp_settings_service_name = rospy.get_param('~read_resp_settings_service_name',
-                                                               'piksi/settings_read_resp')
+                                                               'piksi_multi_base_station/settings_read_resp')
         self.height_base_station_from_ground = rospy.get_param('~height_base_station_from_ground', 0.0)
 
         # Subscribe.
-        rospy.Subscriber(self.spp_topics_name, NavSatFix,
-                         self.navsatfix_spp_callback)
+        rospy.Subscriber(self.navsatfix_topics_name, NavSatFix,
+                         self.navsatfix_callback)
 
         rospy.spin()
 
-    def navsatfix_spp_callback(self, msg):
+    def navsatfix_callback(self, msg):
+        # Sanity check: we should have either SPP or SBAS fix.
+        if msg.status.status != NavSatStatus.STATUS_FIX and msg.status.status != NavSatStatus.STATUS_SBAS_FIX:
+            rospy.logerr(
+                "[navsatfix_callback] received a navsatfix message with status '%d'." % (msg.status.status) +
+                " Accepted status are 'STATUS_FIX' (%d) or 'STATUS_SBAS_FIX' (%d)" % (NavSatStatus.STATUS_FIX, NavSatStatus.STATUS_SBAS_FIX))
+            return
+
         self.latitude_accumulator += msg.latitude
         self.longitude_accumulator += msg.longitude
         self.altitude_accumulator += msg.altitude