Merge pull request #19 from inertialsense/RTK

Rtk

CMakeLists.txt

@@ -4,6 +4,7 @@ project(inertial_sense)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
+  std_msgs
   sensor_msgs
   geometry_msgs
   message_generation
@@ -15,10 +16,16 @@ SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11 -fms-extensions -Wl,--no-as
 
 add_message_files(
   FILES
+  GTime.msg
   SatInfo.msg
   GPS.msg
   GPSInfo.msg
   PreIntIMU.msg
+  RTKInfo.msg
+  RTKRel.msg
+  GlonassEphemeris.msg
+  GNSSEphemeris.msg
+  GNSSObservation.msg
 )
 
 add_service_files(

README.md

@@ -1,13 +1,14 @@
 # inertial_sense
 
-A ROS wrapper for the InertialSense uINS GPS-INS sensor
+
+A ROS wrapper for the InertialSense uINS2 RTK-GPS-INS sensor.
 
 ## NOTICE:
 
-To use this node, you will likely need to update firmware on your uINS [release page](https://github.com/inertialsense/InertialSenseSDK/releases). Download the appropriate `.hex` file and use the `firmware_update` ROS service to update the firmware
+To use this node, you will need to update firmware on your uINS to _at least_  v1.7.0 [release page](https://github.com/inertialsense/InertialSenseSDK/releases). Download the appropriate `.hex` file and use the `firmware_update` ROS service to update the firmware
 
 ``` 
-rosservice call /firmware_update /home/superjax/Download/IS_uINS-3_v1.6.6.0_b2781_2018-06-29_175015.hex
+rosservice call /firmware_update /home/superjax/Download/IS_uINS-3_v1.7.0<...>.hex
 ```
 
 ## Installation
@@ -41,6 +42,17 @@ rosrun inertial_sense inertial_sense_node
 
 For setting parameters and topic remappings from a launch file, refer to the [Roslaunch for Larger Projects](http://wiki.ros.org/roslaunch/Tutorials/Roslaunch%20tips%20for%20larger%20projects) page, or the sample `launch/test.launch` file in this repository.
 
+## RTK
+RTK (Realtime Kinematic) GPS requires two gps receivers, a _base_ and a _rover_.  The GPS observations from the base GPS are sent to the rover and the rover is able to calculate a much more accurate (+/- 3cm) relative position to the base.  This requires a surveyed base position and a relatively high-bandwidth connection to the rover.  The RTK functionality in this node is performed by setting parameters shown below.
+
+It is important that the base position be accurate.  There are two primary methods for getting a surveyed base position.
+
+  1. Find the location of the base on Google Maps (quick and easy, not as accurate)
+  2. Put the base into rover mode with a 3rd-party base station such as a NTRIP caster.  Once the base has RTK fix, the absolute position of the base is accurate to within 3 cm.  Averaging this position over time is usually the most accurate way to get a base position, but takes more effort.
+
+
+Once the base position has been identified, set the `refLLA` of the base uINS to your surveyed position to indicate a surveyed base position.
+
 ## Time Stamps
 
 If GPS is available, all header timestamps are calculated with respect to the GPS clock but are translated into UNIX time to be consistent with the other topics in a ROS network.  If GPS is unvailable, then a constant offset between uINS time and system time is estimated during operation  and is applied to IMU and INS message timestamps as they arrive.  There is often a small drift in these timestamps (on the order of a microsecond per second), due to variance in measurement streams and difference between uINS and system clocks, however this is more accurate than stamping the measurements with ROS time as they arrive.  
@@ -49,12 +61,12 @@ In an ideal setting, there should be no jump in timestamps when GPS is first acq
 
 ## Topics
 
-Topics are enabled and disabled using parameters.  By default, only the `ins/` topic is published to save processor time in serializing unecessary messages.
-- `ins/`(nav_msgs/Odometry)
+Topics are enabled and disabled using parameters.  By default, only the `ins` topic is published to save processor time in serializing unecessary messages.
+- `ins`(nav_msgs/Odometry)
     - full 12-DOF measurements from onboard estimator (pose portion is from inertial to body, twist portion is in body frame)
-- `imu/`(sensor_msgs/Imu)
+- `imu`(sensor_msgs/Imu)
     - Raw Imu measurements from IMU1 (NED frame)
-- `gps/`(inertial_sense/GPS)
+- `gps`(inertial_sense/GPS)
     - unfiltered GPS measurements from onboard GPS unit
 - `gps/info`(inertial_sense/GPSInfo)
     - sattelite information and carrier noise ratio array for each sattelite
@@ -64,6 +76,16 @@ Topics are enabled and disabled using parameters.  By default, only the `ins/` t
     - Raw barometer measurements in kPa
 - `preint_imu` (inertial_sense/DThetaVel)
     - preintegrated coning and sculling integrals of IMU measurements
+- `RTK/info` (inertial_sense/RTKInfo)
+    - information about RTK status
+- `RTK/rel` (inertial_sense/RTKRel)
+    * Relative measurement between RTK base and rover
+- `gps/obs` (inertial_sense/GNSSObservation)
+    * Raw satellite observation (psuedorange and carrier phase)
+- `gps/eph` (inertial_sense/GNSSEphemeris)
+    * Satellite Ephemeris for GPS and Galileo GNSS constellations
+- `gps/geph`
+    * Satellite Ephemeris for Glonass GNSS constellation
 
 ## Parameters
 
@@ -91,6 +113,20 @@ Topics are enabled and disabled using parameters.  By default, only the `ins/` t
    - Flag to stream GPS
 * `~stream_GPS_info`(bool, default: false)
    - Flag to stream GPS info messages
+- `stream_GPS_raw` (bool, default: false)
+   - Flag to stream GPS raw messages
+
+**RTK Configuration**
+* `~RTK_Rover` (bool, default: false)
+  - Enables RTK rover mode (requires base corrections from an RTK base)
+* `~RTK_base` (bool, default: false)
+  - Makes the connected uINS a RTK base station and enables the publishing of corrections
+* `~RTK_server_IP` (string, default: 172.0.0.1)
+  - If operating as base, attempts to create a TCP port on this IP for base corrections, if rover, connects to this IP for corrections.
+* `~RTK_server_port` (int, default: 7777)
+  - If operating as base, creates a TCP connection at this port for base corrections, if rover, connects to this port for corrections.
+* `~RTK_correction_type` (string, default: UBLOX)
+  - If operating with limited bandwidth, choose RTCM3 for a lower bandwidth, but less accurate base corrections,  rover and base must match
 
 **Sensor Configuration**
 * `~INS_rpy` (vector(3), default: {0, 0, 0})
@@ -100,7 +136,7 @@ Topics are enabled and disabled using parameters.  By default, only the `ins/` t
 * `~GPS_ant_xyz` (vector(3), default: {0, 0, 0})
     - The NED translation vector between the INS frame and the GPS antenna (wrt INS frame)
 * `~GPS_ref_lla` (vector(3), default: {0, 0, 0})
-    - The Reference longitude, latitude and altitude for NED calculation in degrees, degrees and meters
+    - The Reference longitude, latitude and altitude for NED calculation in degrees, degrees and meters (use the `set_refLLA` service to update this automatically)
 * `~inclination` (float, default: 1.14878541071)
     - The inclination of earth's magnetic field (radians)
 * `~declination` (float, default: 0.20007290992)
@@ -140,3 +176,5 @@ Topics are enabled and disabled using parameters.  By default, only the `ins/` t
   - Put INS into multi axis magnetometer calibration mode.  This is typically used if the uINS is not mounted to a vehicle, or a lightweight vehicle such as a drone.  Simply rotate the uINS around all axes until the light on the uINS turns blue [more info](http://docs.inertialsense.com/user-manual/Setup_Integration/magnetometer_calibration/)
 - `firmware_update` (inertial_sense/FirmwareUpdate)
   - Updates firmware to the `.hex` file supplied (use absolute filenames)
+* `set_refLLA` (std_srvs/Trigger)
+  - Takes the current estimated position and sets it as the `refLLA`.  Use this to set a base position after a survey, or to zero out the `ins` topic.1

include/inertial_sense.h

@@ -17,9 +17,15 @@
 #include "inertial_sense/GPSInfo.h"
 #include "inertial_sense/PreIntIMU.h"
 #include "inertial_sense/FirmwareUpdate.h"
+#include "inertial_sense/RTKRel.h"
+#include "inertial_sense/RTKInfo.h"
+#include "inertial_sense/GNSSEphemeris.h"
+#include "inertial_sense/GlonassEphemeris.h"
+#include "inertial_sense/GNSSObservation.h"
 #include "nav_msgs/Odometry.h"
 #include "std_srvs/Trigger.h"
 #include "std_msgs/Header.h"
+#include "geometry_msgs/Vector3Stamped.h"
 
 # define GPS_UNIX_OFFSET 315964800 // GPS time started on 6/1/1980 while UNIX time started 1/1/1970 this is the difference between those in seconds
 # define LEAP_SECONDS 18 // GPS time does not have leap seconds, UNIX does (as of 1/1/2017 - next one is probably in 2020 sometime unless there is some crazy earthquake or nuclear blast)
@@ -76,10 +82,17 @@ class InertialSenseROS //: SerialListener
   void IMU_callback(const dual_imu_t* const msg);
 
   ros_stream_t GPS_;
-  void GPS_callback(const gps_nav_t* const msg);
+  ros_stream_t GPS_obs_;
+  ros_stream_t GPS_eph_;
+  void GPS_pos_callback(const gps_pos_t* const msg);
+  void GPS_vel_callback(const gps_vel_t* const msg);
+  void GPS_raw_callback(const gps_raw_t* const msg);
+  void GPS_obs_callback(const obsd_t* const msg);
+  void GPS_eph_callback(const eph_t* const msg);
+  void GPS_geph_callback(const geph_t* const msg);
 
   ros_stream_t GPS_info_;
-  void GPS_Info_callback(const gps_sat_t* const msg);
+  void GPS_info_callback(const gps_sat_t* const msg);
 
   ros_stream_t mag_;
   void mag_callback(const magnetometer_t* const msg);
@@ -98,10 +111,25 @@ class InertialSenseROS //: SerialListener
   ros::ServiceServer mag_cal_srv_;
   ros::ServiceServer multi_mag_cal_srv_;
   ros::ServiceServer firmware_update_srv_;
+  ros::ServiceServer refLLA_set_srv_;
+  bool set_current_position_as_refLLA(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response & res);
   bool perform_mag_cal_srv_callback(std_srvs::Trigger::Request & req, std_srvs::Trigger::Response & res);
   bool perform_multi_mag_cal_srv_callback(std_srvs::Trigger::Request & req, std_srvs::Trigger::Response & res);
   bool update_firmware_srv_callback(inertial_sense::FirmwareUpdate::Request & req, inertial_sense::FirmwareUpdate::Response & res);
 
+  void publishGPS();
+
+  typedef enum
+  {
+    RTK_NONE,
+    RTK_ROVER,
+    RTK_BASE
+  } rtk_state_t;
+  rtk_state_t RTK_state_ = RTK_NONE;
+  ros_stream_t RTK_;
+  void RTK_Misc_callback(const gps_rtk_misc_t* const msg);
+  void RTK_Rel_callback(const gps_rtk_rel_t* const msg);
+
 
   /**
    * @brief ros_time_from_week_and_tow
@@ -134,9 +162,11 @@ class InertialSenseROS //: SerialListener
   bool got_first_message_ = false; // Flag to capture first uINS start time guess
 
   // Data to hold on to in between callbacks
+  double lla_[3];
   sensor_msgs::Imu imu1_msg, imu2_msg;
   nav_msgs::Odometry odom_msg;
   inertial_sense::GPS gps_msg;
+  geometry_msgs::Vector3Stamped gps_velEcef;
   inertial_sense::GPSInfo gps_info_msg;
 
   ros::NodeHandle nh_;

msg/GNSSEphemeris.msg

@@ -0,0 +1,35 @@
+Header header
+int32 sat 		# satellite number
+int32 iode 		# IODE Issue of Data, Ephemeris (ephemeris version)
+int32 iodc 		# IODC Issue of Data, Clock (clock version)
+int32 sva 		# SV accuracy (URA index) IRN-IS-200H p.97            
+int32 svh 		# SV health GPS/QZS (0:ok)            
+int32 week 		# GPS/QZS: gps week, GAL: galileo week
+int32 code 		# GPS/QZS: code on L2 * (00=Invalid, 01 = P Code ON, 11 = C/A code ON, 11 = Invalid) * GAL/CMP: data sources
+int32 flag 		# GPS/QZS: L2 P data flag (indicates that the NAV data stream was commanded OFF on the P-code of the in-phase component of the L2 channel) *  CMP: nav type
+GTime toe 		# Toe
+GTime toc 		# clock data reference time (s) (20.3.4.5)
+GTime ttr 		# T_trans
+float64 A 		# Semi-Major Axis m
+float64 e 		# Eccentricity (no units) 
+float64 i0 		# Inclination Angle at Reference Time (rad)
+float64 OMG0 	# Longitude of Ascending Node of Orbit Plane at Weekly Epoch (rad)
+float64 omg 	# Argument of Perigee (rad)
+float64 M0 		# Mean Anomaly at Reference Time (rad)
+float64 deln 	# Mean Motion Difference From Computed Value (rad)
+float64 OMGd 	# Rate of Right Ascension (rad/s)
+float64 idot 	# Rate of Inclination Angle (rad/s)
+float64 crc 	# Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius
+float64 crs 	# Amplitude of the Sine Harmonic Correction Term to the Orbit Radius (m)
+float64 cuc 	# Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude (rad) 
+float64 cus 	# Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude (rad)
+float64 cic 	# Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination (rad)
+float64 cis 	# Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination (rad)
+float64 toes 	# Reference Time Ephemeris in week (s)
+float64 fit 	# fit interval (h) (0: 4 hours, 1:greater than 4 hours)
+float64 f0 		# SV clock parameters - af0
+float64 f1 		# SV clock parameters - af1
+float64 f2 		# SV clock parameters - af2
+float64[4] tgd 	# group delay parameters: GPS/QZS:tgd[0]=TGD (IRN-IS-200H p.103) * GAL:tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1	* CMP :tgd[0]=BGD1,tgd[1]=BGD2
+float64 Adot 	# Adot for CNAV
+float64 ndot 	# ndot for CNAV

msg/GNSSObservation.msg

@@ -0,0 +1,12 @@
+std_msgs/Header header
+GTime time              # time of observation
+uint8 sat 		# satellite number
+uint8 rcv 		# receiver number
+uint8 SNR 		# Signal Strength (0.25 dBHz)
+uint8 LLI 		# Loss-of-Lock Indicator (bit1=loss-of-lock, bit2=half-cycle-invalid)
+uint8 code 		# code indicator (BeiDou: CODE_L1I, Other: CODE_L1C )
+uint8 qualL 	# Estimated carrier phase measurement standard deviation (0.004 cycles)
+uint8 qualP 	# Estimated pseudorange measurement standard deviation (0.01 m)
+float64 L      	# observation data carrier-phase (cycle)
+float64 P      	# observation data pseudorange (m)
+float32 D      	# observation data doppler frequency (0.002 Hz)

msg/GPS.msg

@@ -17,7 +17,8 @@ int32 cno 								# mean carrier noise ratio (dBHz)
 float64 latitude 						# latitude (degrees) 
 float64 longitude						# longitude (degrees)
 float64 altitude						# height above ellipsoid (not MSL) (m)
-geometry_msgs/Vector3 linear_velocity 	# Velocity (m/s) in NED
+geometry_msgs/Vector3 posEcef           # Position (m) in ECEF
+geometry_msgs/Vector3 velEcef       	# Velocity (m/s) in ECEF
 float32 hMSL							# height above MSL
 float32 hAcc							# horizontal accuracy
 float32 vAcc							# vertical accuracy

msg/GTime.msg

@@ -0,0 +1,2 @@
+int64 time
+float64 sec

msg/GlonassEphemeris.msg

@@ -0,0 +1,14 @@
+int32 sat 	# satellite number 
+int32 iode 	# IODE (0-6 bit of tb field) 
+int32 frq 	# satellite frequency number 
+int32 svh 	# satellite health 
+int32 sva 	# satellite accuracy 
+int32 age 	# satellite age of operation 
+GTime toe 	# epoch of epherides (gpst) 
+GTime tof 	# message frame time (gpst) 
+float64[3] pos 	# satellite position (ecef) (m) 
+float64[3] vel 	# satellite velocity (ecef) (m/s) 
+float64[3] acc 	# satellite acceleration (ecef) (m/s^2) 
+float64 taun 	# SV clock bias (s) 
+float64 gamn 	# relative freq bias 
+float64 dtaun 	# delay between L1 and L2 (s) 

msg/RTKInfo.msg

@@ -0,0 +1,10 @@
+Header header
+
+float32[3] BaseLLA 			# base position in lat-lon-altitude (deg, deg, m)
+uint32 cycle_slip_count 	# number of cycle slips detected
+uint32 roverObs				# number of observations from rover (GPS, Glonass, Gallileo, Beidou, Qzs)
+uint32 baseObs				# number of observations from base (GPS, Glonass, Gallileo, Beidou, Qzs)
+uint32 roverEph				# number of ephemeris messages from rover (GPS, Glonass, Gallileo, Beidou, Qzs)
+uint32 baseEph				# number of ephemeris messages from rover (GPS, Glonass, Gallileo, Beidou, Qzs)
+uint32 baseAntcount			# number of base station antenna position measurements
+

msg/RTKRel.msg

@@ -0,0 +1,6 @@
+Header header
+float32 differential_age 				# Age of differential (seconds)
+float32 ar_ratio 						# Ambiguity resolution ratio factor for validation
+geometry_msgs/Vector3 vector_to_base 	# Vector to base (m) in ECEF - If Compassing enabled, this is the 3-vector from antenna 2 to antenna 1
+float32 distance_to_base 				# Distance to Base (m)
+float32 heading_to_base 				# Angle from north to vectorToBase in local tangent plane. (rad)