├── .gitignore ├── LICENSE ├── README.md ├── init_rovio_npose0 ├── CMakeLists.txt ├── README.md ├── launch │ ├── dummy.txt │ └── init_rovio_npose0.launch ├── matlab │ └── configure_external_antenna_position.m ├── package.xml └── scripts │ ├── add_orientation_offset.py │ ├── bearing_from_mag.py │ ├── constant_yaw_to_imu.py │ └── init_rovio_enu.py └── mav_rtk_gui ├── CMakeLists.txt ├── cfg └── piksi_topic_names_example.yaml ├── launch ├── rtk_info_example.launch ├── rtk_npose0_example.launch └── rtk_npose1_example.launch ├── nodes ├── rtk_info ├── rtk_npose0 └── rtk_npose1 ├── package.xml ├── setup.py └── src └── mav_rtk_gui ├── __init__.py └── gui_frames ├── __init__.py ├── helpers.py ├── msf_frame.py ├── rovio_frame.py ├── rtk_fix_plot_frame.py └── rtk_info_frame.py /.gitignore: -------------------------------------------------------------------------------- 1 | */.idea/* 2 | .idea/* 3 | utils/bags/* 4 | src/*.pyc 5 | */cmake-build-debug 6 | geodetic_survey_bags/*.bag 7 | 8 | *.pyc 9 | init_rovio_npose0/launch/debug_tf_frames.launch 10 | 11 | #QT files 12 | *.files 13 | *.includes 14 | *.creator 15 | *.user 16 | *.config 17 | 18 | # Logs geodetic surveys 19 | piksi_multi_rtk_gps/log_surveys/2*.txt 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, Autonomous Systems Lab, ETH Zurich 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mav_rtk_gps 2 | ====== 3 | 4 | **!!!** 5 | 6 | **Piksi ROS drivers have been moved here: [ethz\_piksi\_ros](https://github.com/ethz-asl/ethz_piksi_ros)** 7 | 8 | **!!!** 9 | 10 | 11 | This repository contains python tools, launch files, and wikis about how to use Piksi Real Time Kinematic (RTK) GPS device on MAVs, i.e. fuse GPS measurements into state estimator. 12 | 13 | Overview 14 | ------ 15 | - [init_rovio_npose0](https://github.com/ethz-asl/mav_rtk_gps/tree/master/init_rovio_npose0) : initialize [Rovio](https://github.com/ethz-asl/rovio) to use external GPS pose measurements, when built with [NPOSE=0](https://github.com/ethz-asl/rovio/wiki/Configuration#build-configuration) 16 | - [mav_rtk_gui](https://github.com/ethz-asl/mav_rtk_gps/tree/master/mav_rtk_gui) : handy Graphical User Interfaces to check the status of RTK fix. 17 | 18 | Installing From Source 19 | ------ 20 | ``` 21 | cd ~/catkin_ws/src 22 | git clone https://github.com/ethz-asl/mav_rtk_gps.git 23 | catkin build init_rovio_npose0 24 | catkin build mav_rtk_gui 25 | ``` 26 | 27 | License 28 | ------- 29 | The source code is released under a [BSD 3-Clause license](https://github.com/ethz-asl/mav_rtk_gps/blob/master/LICENSE). 30 | 31 | Credits 32 | ------- 33 | Marco Tranzatto - ETHZ ASL & RSL - 30 November 2017 34 | 35 | Contact 36 | ------- 37 | Marco Tranzatto marcot(at)ethz.ch 38 | 39 | 40 | Bugs & Feature Requests 41 | ------- 42 | Please report bugs and request features using the [Issue Tracker](https://github.com/ethz-asl/mav_rtk_gps/issues). 43 | -------------------------------------------------------------------------------- /init_rovio_npose0/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(init_rovio_npose0) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED 6 | COMPONENTS 7 | rospy 8 | std_msgs 9 | std_srvs 10 | geometry_msgs 11 | rovio 12 | tf 13 | ) 14 | 15 | ################################### 16 | ## catkin specific configuration ## 17 | ################################### 18 | ## The catkin_package macro generates cmake config files for your package 19 | ## Declare things to be passed to dependent projects 20 | ## INCLUDE_DIRS: uncomment this if your package contains header files 21 | ## LIBRARIES: libraries you create in this project that dependent projects also need 22 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 23 | ## DEPENDS: system dependencies of this project that dependent projects also need 24 | catkin_package( 25 | # INCLUDE_DIRS 26 | # LIBRARIES 27 | CATKIN_DEPENDS 28 | std_msgs 29 | std_srvs 30 | geometry_msgs 31 | rovio 32 | tf 33 | piksi_multi_rtk_gps 34 | # DEPENDS 35 | ) 36 | -------------------------------------------------------------------------------- /init_rovio_npose0/README.md: -------------------------------------------------------------------------------- 1 | init_rovio_npose0 2 | ====== 3 | 4 | Reset Rovio to allow fusing GPS external pose measurements and visual inertial odometry. 5 | In order to do that, it is mandatory to align (same orientation) GPS frame and Rovio odometry frame. 6 | 7 | 8 | GPS measurements are assumed to be expressed in a local ENU (East-North-Up) frame. 9 | If you have Navigation Satellite fix measurements (Latitude, Longitude and Altitude) you can use the repo [Geodetic Utils](https://github.com/ethz-asl/geodetic_utils) to convert them in local ENU. 10 | 11 | Parameters `pose_sensor/init/q_*` and `pose_sensor/init/p_*` can be obtained with [Kalibr](https://github.com/ethz-asl/kalibr). 12 | 13 | Dependencies 14 | ------ 15 | * [ethz\_piksi\_ros](https://github.com/ethz-asl/ethz_piksi_ros) 16 | 17 | Published and subscribed topics/services 18 | ------ 19 | 20 | - Subscribed topics: 21 | - **`init_rovio_enu/mag_imu`** of type `sensor_msgs/Imu Message`. This is the orientation of the MAV IMU with respect to local ENU frame (i.e., **`yaw` is with respect to East axis**). 22 | - **`init_rovio_enu/gps_transform`** of type `geometry_msgs/TransformStamped`. This is the transformation (rotation part does not really matter in this case) from local ENU frame to GPS antenna. 23 | - **`init_rovio_enu/gps_pose`** of type `geometry_msgs/PoseWithCovarianceStamped`. This is the pose (rotation part does not really matter in this case) from local ENU frame to GPS antenna. (use either `init_rovio_enu/gps_transform` or `init_rovio_enu/gps_pose`). 24 | 25 | - Advertised services: 26 | - **`init_rovio_enu/send_reset_to_rovio`** of type `std_srvs/Trigger`. This resets Rovio internal state and alignes Rovio odometry frame to local ENU frame. 27 | 28 | Parameters 29 | ------ 30 | A summary of the parameters: 31 | 32 | | Parameter | Description | 33 | | -------------------- |:-------------------------------------------------------------------------------:| 34 | | `mavimu_p_mavimu_gps` | position of GPS antenna from MAV IMU, expressed in IMU MAV. | 35 | | `send_reset_automatically` | should reset to Rovio be sent automatically? | 36 | | `samples_before_reset` | # samples of `mag_imu` after which automatic reset is sent | 37 | | `verbose` | verbose output of the node | 38 | | `pose_sensor/init/q_ic/w` | W of quaternion from IMU of the camera-sensor to MAV IMU. | 39 | | `pose_sensor/init/q_ic/x` | X of quaternion from IMU of the camera-sensor to MAV IMU. | 40 | | `pose_sensor/init/q_ic/y` | Y of quaternion from IMU of the camera-sensor to MAV IMU. | 41 | | `pose_sensor/init/q_ic/z` | Z of quaternion from IMU of the camera-sensor to MAV IMU. | 42 | | `pose_sensor/init/p_ic/x` | X of position of IMU of the camera-sensor from MAV IMU, expressed in MAV IMU. | 43 | | `pose_sensor/init/p_ic/y` | Y of position of IMU of the camera-sensor from MAV IMU, expressed in MAV IMU. | 44 | | `pose_sensor/init/p_ic/z` | Z of position of IMU of the camera-sensor from MAV IMU, expressed in MAV IMU. | 45 | 46 | Contact 47 | ------- 48 | Marco Tranzatto marcot(at)ethz.ch 49 | -------------------------------------------------------------------------------- /init_rovio_npose0/launch/dummy.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/mav_rtk_gps/94f97b24ec30c58b2632cbd4ed7becbe6a3f0b6d/init_rovio_npose0/launch/dummy.txt -------------------------------------------------------------------------------- /init_rovio_npose0/launch/init_rovio_npose0.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | [0.0, 0.0, 0.11] 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /init_rovio_npose0/matlab/configure_external_antenna_position.m: -------------------------------------------------------------------------------- 1 | %% 2 | % This script helps you configuring rovio_filter.info file when using 3 | % external pose measurements, such as a GPS receiver. 4 | % The output of this script is a quaterion (qVM) and 5 | % a position vector (MrMV), which must be copied into 6 | % 'rovio_filter.info' file, in the section "PoseUpdate". 7 | % 8 | % Script outputs: 9 | % qVM represents the orientation of the external pose measurement frame 10 | % with respect to ViSensor IMU frame. 11 | % MrMV represents the position of the origin of the external pose 12 | % measurement frame with respect to the origin of the ViSensor IMU frame. 13 | % 14 | % Frames: 15 | % Frame names in rovio: M (ViSensor IMU), V (external pose, for example 16 | % GPS), I (inertial) 17 | % Frame names in msf: I (MAV IMU), C (ViSensor IMU) 18 | % Custon frame names used in this script: MI (MAV IMU) 19 | % 20 | 21 | % ---------------------------------------------------------- 22 | %% user's parameters: fill this section with our parameters! 23 | % ---------------------------------------------------------- 24 | 25 | % msf_parameters_vision from msf_parameters_vision.yaml file 26 | % pose_sensor/init/q_ic 27 | q_I_C_x = -0.58546635941; 28 | q_I_C_y = 0.588750056004; 29 | q_I_C_z = -0.389971319019; 30 | q_I_C_w = 0.398151835231; 31 | 32 | % pose_sensor/init/p_ic 33 | I_p_I_C_x = 0.120477158968; 34 | I_p_I_C_y = -0.00541789178186; 35 | I_p_I_C_z = -0.109118925469; 36 | 37 | % gps antenna position respect MAV IMU, for now taken manually with a meter 38 | MI_p_MI_V_x = 0.0; 39 | MI_p_MI_V_y = 0.0; 40 | MI_p_MI_V_z = 0.11; 41 | 42 | % ---------------------------------------------------------- 43 | 44 | %% compute transformation T_MI_M 45 | T_MI_V = trvec2tform([MI_p_MI_V_x, MI_p_MI_V_y, MI_p_MI_V_z]); 46 | % simple inverse of T_MI_V since there's no rotation 47 | T_V_MI = trvec2tform(-[MI_p_MI_V_x, MI_p_MI_V_y, MI_p_MI_V_z]); 48 | % q = [w x y z] 49 | T_MI_M = trvec2tform([I_p_I_C_x, I_p_I_C_y, I_p_I_C_z]) * ... 50 | quat2tform([q_I_C_w, q_I_C_x, q_I_C_y, q_I_C_z]); 51 | 52 | T_V_M = T_V_MI * T_MI_M; 53 | q_V_M = tform2quat(T_V_M); 54 | R_V_M = tform2rotm(T_V_M); 55 | V_r_V_M = tform2trvec(T_V_M); 56 | 57 | T_M_V = [R_V_M', -(R_V_M') * V_r_V_M'; 58 | 0.0, 0.0, 0.0, 1.0]; 59 | M_r_M_V = tform2trvec(T_M_V); 60 | q_M_V = tform2quat(T_M_V); 61 | 62 | %% print rovio settings for external pose section 63 | % q = [w x y z] 64 | fprintf(['\n ----- copy following values into section "PoseUpdate"' ... 65 | ' of rovio_filter.info ----- \n\n']); 66 | 67 | fprintf(' qVM_x %.7f \n qVM_y %.7f \n qVM_z %.7f \n qVM_w %.7f \n', ... 68 | q_V_M(2), q_V_M(3), q_V_M(4), q_V_M(1)); 69 | fprintf(' MrMV_x %.7f \n MrMV_y %.7f \n MrMV_z %.7f \n', ... 70 | M_r_M_V(1), M_r_M_V(2), M_r_M_V(3)); 71 | 72 | fprintf(['\n-------------------------------------------------------' ... 73 | '------------------------------\n']); 74 | 75 | %% print TF deug broadcaster nodes 76 | % creat TF broadcaster to visualize transformation in rviz 77 | fprintf(['\n ----- use the following TF broadcaster to double ' ... 78 | ' check outputed transformation ----- \n\n']); 79 | % map == mav_imu for testing 80 | fprintf([' \n']); 84 | 85 | % mav_imu to ViSensor imu 86 | tf_node_string=[' \n']; 90 | fprintf(tf_node_string, I_p_I_C_x, I_p_I_C_y, I_p_I_C_z, ... 91 | q_I_C_x, q_I_C_y, q_I_C_z, q_I_C_w); 92 | 93 | % ViSensor imu to GPS antenna position (T_M_V) 94 | tf_node_string=[' \n']; 98 | 99 | px = M_r_M_V(1); 100 | py = M_r_M_V(2); 101 | pz = M_r_M_V(3); 102 | qx = q_M_V(2); 103 | qy = q_M_V(3); 104 | qz = q_M_V(4); 105 | qw = q_M_V(1); 106 | fprintf(tf_node_string, px, py, pz, qx, qy, qz, qw); 107 | fprintf(['\n-------------------------------------------------------' ... 108 | '------------------------------\n']); -------------------------------------------------------------------------------- /init_rovio_npose0/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | init_rovio_npose0 4 | 0.0.0 5 | 6 | Nodes to initiliaze ROVIO when compiled with option NPOSE=0. 7 | 8 | 9 | Marco Tranzatto 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | rospy 16 | std_msgs 17 | std_srvs 18 | geometry_msgs 19 | rovio 20 | tf 21 | piksi_multi_rtk_gps 22 | 23 | 24 | -------------------------------------------------------------------------------- /init_rovio_npose0/scripts/add_orientation_offset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: add_orientation_offset.py 5 | # Description: ROS module to add a constant offset to incoming IMU messages. 6 | # WARNING, use this module ONLY to initialize Rovio with NPOSE=0, as ONLY IMU orientation is processed, 7 | # accelerations and gyros are not affected!!! 8 | # Convention: q_out = q_in * q_offset 9 | 10 | import rospy 11 | from sensor_msgs.msg import Imu 12 | import tf.transformations as tf 13 | import math 14 | 15 | 16 | class AddOrientationOffset: 17 | def __init__(self): 18 | 19 | if rospy.has_param('~orientation_offset'): 20 | # Orientation offset as quaterion q = [x,y,z,w]. 21 | self.orientation_offset = rospy.get_param('~orientation_offset') 22 | else: 23 | yaw_offset_deg = rospy.get_param('~yaw_offset_deg', 0.0) 24 | self.orientation_offset = tf.quaternion_from_euler(0.0, 0.0, math.radians(yaw_offset_deg)) 25 | 26 | rospy.Subscriber(rospy.get_name() + "/imu_in", Imu, self.imu_callback) 27 | 28 | self.pub_imu_out = rospy.Publisher(rospy.get_name() + '/imu_out', 29 | Imu, queue_size=10) 30 | 31 | rospy.spin() 32 | 33 | def imu_callback(self, msg): 34 | 35 | q_in = [msg.orientation.x, 36 | msg.orientation.y, 37 | msg.orientation.z, 38 | msg.orientation.w] 39 | 40 | q_out = tf.quaternion_multiply(q_in, self.orientation_offset) 41 | 42 | imu_out = msg 43 | imu_out.orientation.x = q_out[0] 44 | imu_out.orientation.y = q_out[1] 45 | imu_out.orientation.z = q_out[2] 46 | imu_out.orientation.w = q_out[3] 47 | 48 | self.pub_imu_out.publish(imu_out) 49 | 50 | 51 | if __name__ == '__main__': 52 | rospy.init_node('add_orientation_offset') 53 | rospy.loginfo(rospy.get_name() + ' start') 54 | 55 | # Go to class functions that do all the heavy lifting. Do error checking. 56 | try: 57 | add_orientation_offset = AddOrientationOffset() 58 | except rospy.ROSInterruptException: 59 | pass 60 | -------------------------------------------------------------------------------- /init_rovio_npose0/scripts/bearing_from_mag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: bearing_from_mag.py 5 | # Description: ROS module to calculate bearing angle from magnetometer data, when the MAV is placed 6 | # horizontally, i.e. WITH pitch == roll == 0 (or at least very small). 7 | # 8 | 9 | import rospy 10 | import tf.transformations as tf 11 | from std_msgs.msg import Float64 12 | from geometry_msgs.msg import Vector3Stamped 13 | from sensor_msgs.msg import Imu 14 | import numpy as np 15 | import math 16 | 17 | class BearingFromMag(): 18 | def __init__(self): 19 | # Read Settings 20 | self.read_settings() 21 | 22 | # Init other variables 23 | self._num_magnetometer_reads = 0 24 | self._latest_bearings = np.zeros(shape = (self._number_samples_average, 1)) 25 | self._received_enough_samples = False 26 | 27 | # Subscribe to magnetometer topic 28 | rospy.Subscriber("magnetic_field", Vector3Stamped, self.magnetic_field_callback) 29 | 30 | # Publishers 31 | self._pub_bearing_raw = rospy.Publisher(rospy.get_name() + '/bearing_raw_deg', 32 | Float64, queue_size = 10) 33 | self._pub_bearing_avg = rospy.Publisher(rospy.get_name() + '/bearing_avg_deg', 34 | Float64, queue_size = 10) 35 | self._pub_imu_bearing_avg = rospy.Publisher(rospy.get_name() + '/imu_bearing_avg', 36 | Imu, queue_size = 10) 37 | 38 | if self._verbose: 39 | self._pub_mag_corrected = rospy.Publisher(rospy.get_name() + '/mag_corrected', 40 | Vector3Stamped, queue_size = 10) 41 | 42 | rospy.spin() 43 | 44 | def read_settings(self): 45 | # Declination 46 | self._declination = math.radians(rospy.get_param('~declination_deg', 0.0)) 47 | 48 | # Calibration offset 49 | self._calibration_offset = np.array(rospy.get_param('~calibration_offset', [0.0, 0.0, 0.0])) 50 | 51 | # Calibration compensation 52 | self._calibration_compensation = np.array(rospy.get_param('~calibration_compensation', 53 | [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0])) 54 | # create matrix from array 55 | self._calibration_compensation = self._calibration_compensation.reshape(3,3) 56 | 57 | # Constant bearing offset 58 | self._bearing_offset = math.radians(rospy.get_param('~bearing_constant_offset_deg', 0.0)) 59 | 60 | # Number of samples used to compute average 61 | self._number_samples_average = rospy.get_param('~number_samples_average', 10) 62 | 63 | # Verbose 64 | self._verbose = rospy.get_param('~verbose', "True") 65 | 66 | # Print some useful information 67 | rospy.logwarn(rospy.get_name() + 68 | " declination: " + 69 | str(math.degrees(self._declination)) + " (deg)") 70 | 71 | rospy.logwarn(rospy.get_name() + 72 | " constant bearing offset added to final bearing: " + 73 | str(math.degrees(self._bearing_offset)) + " (deg)") 74 | 75 | if self._verbose: 76 | rospy.loginfo(rospy.get_name() + 77 | " calibration offset: " + str(self._calibration_offset)) 78 | rospy.loginfo(rospy.get_name() + 79 | " calibration compensation: \n" + 80 | str(self._calibration_compensation)) 81 | rospy.loginfo(rospy.get_name() + 82 | " number of samples to average: " + 83 | str(self._number_samples_average)) 84 | 85 | def magnetic_field_callback(self, magnetometer_msg): 86 | 87 | # Correct magnetic filed 88 | raw_mag = np.array([magnetometer_msg.vector.x, 89 | magnetometer_msg.vector.y, 90 | magnetometer_msg.vector.z]) 91 | 92 | # corrected_mag = compensation * (raw_mag - offset) 93 | corrected_mag = np.dot(self._calibration_compensation, 94 | raw_mag - self._calibration_offset) 95 | 96 | # compute yaw angle using corrected magnetometer measurements 97 | # and ASSUMING ZERO pitch and roll of the magnetic sensor! 98 | # adapted from 99 | # https://github.com/KristofRobot/razor_imu_9dof/blob/indigo-devel/src/Razor_AHRS/Compass.ino 100 | corrected_mag = corrected_mag / np.linalg.norm(corrected_mag) 101 | mag_bearing = math.atan2(corrected_mag[1], -corrected_mag[0]) 102 | 103 | # add declination and constant bearing offset 104 | mag_bearing = mag_bearing + self._declination + self._bearing_offset 105 | 106 | # publish unfiltered bearing, degrees only for debug purposes 107 | self._pub_bearing_raw.publish(Float64(math.degrees(mag_bearing))) 108 | 109 | # compute mean 110 | self._latest_bearings[self._num_magnetometer_reads] = mag_bearing 111 | self._num_magnetometer_reads += 1 112 | 113 | if self._num_magnetometer_reads >= self._number_samples_average: 114 | self._num_magnetometer_reads = 0 # delete oldest samples 115 | self._received_enough_samples = True 116 | 117 | if self._received_enough_samples: 118 | bearing_avg = self.angular_mean(self._latest_bearings) 119 | else: 120 | # not enough samples, use latest value 121 | bearing_avg = mag_bearing 122 | 123 | # WARNING: we assume zero roll and zero pitch! 124 | q_avg = tf.quaternion_from_euler(0.0, 0.0, bearing_avg); 125 | imu_msg = Imu() 126 | imu_msg.orientation.w = q_avg[3] 127 | imu_msg.orientation.x = q_avg[0] 128 | imu_msg.orientation.y = q_avg[1] 129 | imu_msg.orientation.z = q_avg[2] 130 | 131 | self._pub_bearing_avg.publish(Float64(math.degrees(bearing_avg))) 132 | self._pub_imu_bearing_avg.publish(imu_msg) 133 | 134 | # debug 135 | if self._verbose: 136 | rospy.loginfo("bearing_avg : " + 137 | str(math.degrees(bearing_avg)) + " deg") 138 | 139 | mag_corrected_msg = magnetometer_msg 140 | mag_corrected_msg.vector.x = corrected_mag[0] 141 | mag_corrected_msg.vector.y = corrected_mag[1] 142 | mag_corrected_msg.vector.z = corrected_mag[2] 143 | self._pub_mag_corrected.publish(mag_corrected_msg) 144 | 145 | def angular_mean(self, angles_array): 146 | # choose one of the following 147 | return self.atan2_mean(angles_array) 148 | #TODO (marco-tranzatto) remove once above has been tested 149 | #return self.mitsuta_mean(angles_array) 150 | 151 | # From Wikipedia: 152 | # https://en.wikipedia.org/wiki/Mean_of_circular_quantities 153 | def atan2_mean(self, angles_array): 154 | sum_sin = 0.0 155 | sum_cos = 0.0 156 | 157 | for angle in angles_array: 158 | sum_sin += math.sin(angle) 159 | sum_cos += math.cos(angle) 160 | 161 | return math.atan2(sum_sin, sum_cos) 162 | 163 | # Mitsuta mean used to average angles. This is necessary in order to avoid 164 | # misleading behaviours. For example, if the measurements are swtiching between 165 | # -180 and +180 (they are the same angle, just with differente representation) 166 | # then a normal mean algorithm would give you 0, which is completely wrong. 167 | # Code adapted from: 168 | # https://github.com/SodaqMoja/Mitsuta/blob/master/mitsuta.py 169 | def mitsuta_mean(self, angles_array): 170 | # Function meant to work with degrees, covert inputs 171 | # from radians to degrees and output from degrees to radians 172 | D = math.degrees(angles_array[0]) 173 | mysum = D 174 | for val in angles_array[1:]: 175 | val = math.degrees(val) 176 | delta = val - D 177 | if delta < -180.0: 178 | D = D + delta + 360.0 179 | elif delta < 180.0: 180 | D = D + delta 181 | else: 182 | D = D + delta - 360.0 183 | mysum = mysum + D 184 | m = mysum / len(angles_array) 185 | 186 | avg = math.radians((m + 360.0) % 360.0) 187 | # make sure avg is between -pi and pi 188 | if avg > math.pi: 189 | avg = avg - 2.0 * math.pi 190 | elif avg < -math.pi: 191 | avg = avg + 2.0 * math.pi 192 | 193 | return avg 194 | 195 | if __name__ == '__main__': 196 | 197 | rospy.init_node('bearing_from_mag') 198 | rospy.loginfo(rospy.get_name() + " start") 199 | 200 | # Go to class functions that do all the heavy lifting. Do error checking. 201 | try: 202 | bearing_from_mag = BearingFromMag() 203 | except rospy.ROSInterruptException: pass 204 | -------------------------------------------------------------------------------- /init_rovio_npose0/scripts/constant_yaw_to_imu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: constant_yaw_to_imu.py 5 | # Description: ROS module to publish imu messages built from a constant yaw angle. 6 | # 7 | 8 | import rospy 9 | import tf.transformations as tf 10 | from sensor_msgs.msg import Imu 11 | import math 12 | 13 | def yaw_to_imu(yaw): 14 | 15 | # WARNING: we assume zero roll and zero pitch! 16 | q_avg = tf.quaternion_from_euler(0.0, 0.0, yaw); 17 | imu_msg = Imu() 18 | imu_msg.orientation.w = q_avg[3] 19 | imu_msg.orientation.x = q_avg[0] 20 | imu_msg.orientation.y = q_avg[1] 21 | imu_msg.orientation.z = q_avg[2] 22 | 23 | pub_imu.publish(imu_msg) 24 | 25 | if __name__ == '__main__': 26 | 27 | rospy.init_node('constant_yaw_to_imu') 28 | rospy.loginfo(rospy.get_name() + " start") 29 | 30 | # Read Settings 31 | publishing_frequency = rospy.get_param('~publishing_frequency', 2.0) 32 | constant_yaw = math.radians(rospy.get_param('~constant_yaw_deg', 0.0)) 33 | 34 | pub_imu = rospy.Publisher(rospy.get_name() + '/imu', 35 | Imu, queue_size = 10) 36 | 37 | rate = rospy.Rate(publishing_frequency) 38 | while not rospy.is_shutdown(): 39 | yaw_to_imu(constant_yaw) 40 | rate.sleep() 41 | -------------------------------------------------------------------------------- /init_rovio_npose0/scripts/init_rovio_enu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: init_rovio_state.py 5 | # Description: ROS module to initialize Rovio world frame and align it with local ENU frame. 6 | # Convention: Quaternion q_A_B: r_A = rotation_matrix(q_A_B) * r_B 7 | # Vector A_p_B_C: vector expressed in frame A, from point B to point C 8 | # Frames: I = MAV IMU; C = Sensor IMU; Enu = East-North-Up 9 | # 10 | 11 | import rospy 12 | import tf.transformations as tf 13 | from std_msgs.msg import * 14 | import std_srvs.srv 15 | from geometry_msgs.msg import TransformStamped, Pose, PoseWithCovarianceStamped 16 | from sensor_msgs.msg import Imu 17 | from rovio.srv import SrvResetToPose 18 | import math 19 | import os 20 | import numpy as np 21 | 22 | class InitRovioEnu: 23 | 24 | def __init__(self): 25 | # read settings 26 | self._samples_before_reset = rospy.get_param('~samples_before_reset', 50) 27 | self._send_reset_automatically = rospy.get_param('~send_reset_automatically', False) 28 | # this is a crucial node, be verbose per default 29 | self._verbose = rospy.get_param('~verbose', True) 30 | 31 | # quaternion from IMU of the camera-sensor (C frame) to IMU of the MAV (I frame) 32 | q_I_C_w = rospy.get_param('~pose_sensor/init/q_ic/w', 1.0) 33 | q_I_C_x = rospy.get_param('~pose_sensor/init/q_ic/x', 0.0) 34 | q_I_C_y = rospy.get_param('~pose_sensor/init/q_ic/y', 0.0) 35 | q_I_C_z = rospy.get_param('~pose_sensor/init/q_ic/z', 0.0) 36 | q_I_C = [q_I_C_x, q_I_C_y, q_I_C_z, q_I_C_w] 37 | 38 | # position of IMU of the camera-sensor (C frame) from IMU of the MAV (I frame) 39 | # yes, it's the other way around respect the quaternion ... 40 | I_p_I_C_x = rospy.get_param('~pose_sensor/init/p_ic/x', 0.0) 41 | I_p_I_C_y = rospy.get_param('~pose_sensor/init/p_ic/y', 0.0) 42 | I_p_I_C_z = rospy.get_param('~pose_sensor/init/p_ic/z', 0.0) 43 | I_p_I_C = [I_p_I_C_x, I_p_I_C_y, I_p_I_C_z] 44 | 45 | # position of GPS antenna (V frame, accordin to Rovio) 46 | # with respect to MAV IMU (I frame, accordin to MSF) 47 | self._I_p_I_V = rospy.get_param('~mavimu_p_mavimu_gps', [0.0, 0.0, 0.0]) 48 | 49 | # full transformation from MAV IMU (I) to sensor IMU (C) 50 | # do first translation and then rotation! 51 | self._T_I_C = tf.concatenate_matrices(tf.translation_matrix(I_p_I_C), 52 | tf.quaternion_matrix(q_I_C)) 53 | 54 | if self._verbose: 55 | rospy.loginfo(rospy.get_name() + 56 | ": transformation from vi-sensor IMU and MAV IMU [x, y, z, w]: " + 57 | str(q_I_C)) 58 | 59 | if self._send_reset_automatically: 60 | rospy.loginfo(rospy.get_name() + 61 | ": reset will be sent after " + 62 | str(self._samples_before_reset) + " of IMU messages") 63 | else: 64 | rospy.loginfo(rospy.get_name() + 65 | ": reset has to be sent by calling service 'send_reset_to_rovio' ") 66 | 67 | # init other variables 68 | self._num_imu_msgs_read = 0 69 | self._num_gps_transform_msgs_read = 0 70 | self._Enu_p_Enu_V = [0.0, 0.0, 0.0] 71 | self._automatic_rovio_reset_sent_once = False 72 | self._pose_world_imu_msg = Pose() 73 | self._T_Enu_I = tf.identity_matrix() 74 | 75 | # allow testing in Vicon 76 | testing_in_vicon = rospy.get_param('~testing_in_vicon', False) 77 | if testing_in_vicon: 78 | # manually set this paramter if testing in Vicon 79 | # it will be overwritten in any case after the first external GPS measurement 80 | self._Enu_p_Enu_V = rospy.get_param('~vicon_p_vicon_gps', [0.0, 0.0, 0.0]) 81 | self._num_gps_transform_msgs_read = 1 82 | 83 | # advertise service 84 | self._reset_rovio_srv_server = rospy.Service(rospy.get_name() + 85 | '/send_reset_to_rovio', 86 | std_srvs.srv.Trigger, 87 | self.send_reset_to_rovio_service_callback) 88 | 89 | # subscribe to Imu topic which contains the yaw orientation 90 | rospy.Subscriber(rospy.get_name() + "/mag_imu", Imu, self.mag_imu_callback) 91 | # use either gps_transform or gps_pose 92 | rospy.Subscriber(rospy.get_name() + "/gps_transform", TransformStamped, 93 | self.gps_transform_callback) 94 | rospy.Subscriber(rospy.get_name() + "/gps_pose", PoseWithCovarianceStamped, 95 | self.gps_pose_callback) 96 | 97 | rospy.spin() 98 | 99 | def gps_pose_callback(self, pose_msg): 100 | self._num_gps_transform_msgs_read += 1 101 | self._Enu_p_Enu_V = [ pose_msg.pose.pose.position.x, 102 | pose_msg.pose.pose.position.y, 103 | pose_msg.pose.pose.position.z] 104 | 105 | def gps_transform_callback(self, gps_msg): 106 | self._num_gps_transform_msgs_read += 1 107 | self._Enu_p_Enu_V = [ gps_msg.transform.translation.x, 108 | gps_msg.transform.translation.y, 109 | gps_msg.transform.translation.z] 110 | 111 | def mag_imu_callback(self, imu_msg): 112 | self._num_imu_msgs_read += 1 113 | 114 | # compute new pose to use when resetting ROVIO 115 | # orientation of the IMU frame of the MAV (body frame, or I frame according to MSF) 116 | q_Enu_I = [imu_msg.orientation.x, 117 | imu_msg.orientation.y, 118 | imu_msg.orientation.z, 119 | imu_msg.orientation.w] 120 | R_Enu_I = tf.quaternion_matrix(q_Enu_I) 121 | 122 | # use latest position received from GPS, but first compute position of MAV IMU from GPS 123 | # by subtracting offset between MAV IMU and GPS antenna 124 | I_p_I_V = np.array(self._I_p_I_V) 125 | Enu_p_I_V = np.dot(R_Enu_I[0:3, 0:3], I_p_I_V) 126 | Enu_p_Enu_I = self._Enu_p_Enu_V - Enu_p_I_V 127 | 128 | # full transformation from MAV IMU (I) to local ENU (East-North-Up) frame 129 | # do first translation and then rotation! 130 | self._T_Enu_I = tf.concatenate_matrices(tf.translation_matrix(Enu_p_Enu_I), 131 | R_Enu_I) 132 | 133 | if self._send_reset_automatically and not self._automatic_rovio_reset_sent_once and \ 134 | self._num_imu_msgs_read > self._samples_before_reset and self._num_gps_transform_msgs_read > 0: 135 | (success, message) = self.send_reset_to_rovio() 136 | 137 | if success: 138 | self._automatic_rovio_reset_sent_once = True 139 | 140 | rospy.loginfo(rospy.get_name() + " " + message) 141 | 142 | def send_reset_to_rovio_service_callback(self, request): 143 | response = std_srvs.srv.TriggerResponse() 144 | 145 | if self._automatic_rovio_reset_sent_once or self._send_reset_automatically: 146 | message = "Reset sent automatically after %d IMU messages, rosservice call refused." % \ 147 | (self._samples_before_reset) 148 | 149 | rospy.logwarn(rospy.get_name() + " " + message) 150 | response.success = False 151 | response.message = message 152 | elif self._num_imu_msgs_read <= 0: 153 | response.success = False 154 | response.message = "No external IMU message received, at least one single orientation is needed." 155 | elif self._num_gps_transform_msgs_read <= 0: 156 | response.success = False 157 | response.message = "No external GPS transform message received, at least one single position is needed." 158 | else: # everything's fine, send reset 159 | (success, message) = self.send_reset_to_rovio() 160 | response.success = success 161 | response.message = message 162 | 163 | return response 164 | 165 | def send_reset_to_rovio(self): 166 | rospy.wait_for_service('rovio/reset_to_pose') 167 | 168 | try: 169 | rovio_reset_srv = rospy.ServiceProxy('rovio/reset_to_pose', SrvResetToPose) 170 | 171 | # compute pose from local ENU (East-North-Up frame) to 172 | # IMU frame of the ViSensor (== C frame, according to MSF) 173 | T_Enu_C = tf.concatenate_matrices(self._T_Enu_I, self._T_I_C) 174 | q_Enu_C = tf.quaternion_from_matrix(T_Enu_C) 175 | 176 | # set new Sensor IMU position and orientation respect to World frame 177 | # (which is now aligned to local ENU) 178 | self._pose_world_imu_msg.position.x = T_Enu_C[0, 3] 179 | self._pose_world_imu_msg.position.y = T_Enu_C[1, 3] 180 | self._pose_world_imu_msg.position.z = T_Enu_C[2, 3] 181 | self._pose_world_imu_msg.orientation.w = q_Enu_C[3] 182 | self._pose_world_imu_msg.orientation.x = q_Enu_C[0] 183 | self._pose_world_imu_msg.orientation.y = q_Enu_C[1] 184 | self._pose_world_imu_msg.orientation.z = q_Enu_C[2] 185 | 186 | rovio_reset_srv(self._pose_world_imu_msg) 187 | 188 | if self._verbose: 189 | q_Enu_I = tf.quaternion_from_matrix(self._T_Enu_I) 190 | (yaw, pitch, roll) = tf.euler_from_quaternion(q_Enu_I, 'rzyx') 191 | rospy.loginfo(rospy.get_name() + ": body frame of MAV assumed with " + 192 | str(math.degrees(roll)) + " (deg) roll, " + 193 | str(math.degrees(pitch)) + " (deg) pitch, " + 194 | str(math.degrees(yaw)) + " (deg) yaw from local ENU (local axis, ZYX)") 195 | 196 | self.create_rovio_init_info() 197 | 198 | success = True 199 | message = "Service call to reset Rovio internal state sent" 200 | 201 | return (success, message) 202 | 203 | except rospy.ServiceException, e: 204 | success = False 205 | message = "Service call to reset Rovio internal state failed: %s"%e 206 | return (success, message) 207 | 208 | def create_rovio_init_info(self): 209 | # Debugging file for Rviz 210 | # current path of init_rovio_enu.py file 211 | script_path = os.path.dirname(os.path.realpath(sys.argv[0])) 212 | # write debug tf launch file in parent launch folder of parent directory 213 | desired_path = "%s/../launch/debug_tf_frames.launch" % (script_path) 214 | file_obj = open(desired_path, 'w') 215 | file_obj.write(" \n") 216 | file_obj.write(" \n \n") 217 | # final pose of Sensor IMU used to reset Rovio 218 | self.create_tf_debug_node("ENU", "sensor_imu_rovio", 219 | self._pose_world_imu_msg.position.x, 220 | self._pose_world_imu_msg.position.y, 221 | self._pose_world_imu_msg.position.z, 222 | self._pose_world_imu_msg.orientation.x, 223 | self._pose_world_imu_msg.orientation.y, 224 | self._pose_world_imu_msg.orientation.z, 225 | self._pose_world_imu_msg.orientation.w, 226 | file_obj) 227 | 228 | q_Enu_I = tf.quaternion_from_matrix(self._T_Enu_I) 229 | self.create_tf_debug_node("ENU", "mav_imu", 230 | self._T_Enu_I[0, 3], 231 | self._T_Enu_I[1, 3], 232 | self._T_Enu_I[2, 3], 233 | q_Enu_I[0], 234 | q_Enu_I[1], 235 | q_Enu_I[2], 236 | q_Enu_I[3], 237 | file_obj) 238 | 239 | # following tf should overlap with enu_to_sensor_imu 240 | # full Sensor IMU posed form msf_parameters_vision 241 | q_I_C = tf.quaternion_from_matrix(self._T_I_C) 242 | self.create_tf_debug_node("mav_imu", "sensor_imu_check", 243 | self._T_I_C[0, 3], 244 | self._T_I_C[1, 3], 245 | self._T_I_C[2, 3], 246 | q_I_C[0], 247 | q_I_C[1], 248 | q_I_C[2], 249 | q_I_C[3], 250 | file_obj) 251 | 252 | file_obj.write("\n \n\n") 253 | file_obj.close() 254 | 255 | def create_tf_debug_node(self, frame_id, child_frame_id, x, y, z, qx, qy, qz, qw, file_obj): 256 | buffer = "" % ( 258 | frame_id, child_frame_id, 259 | x, y, z, 260 | qx, qy, qz, qw, 261 | frame_id, child_frame_id) 262 | 263 | file_obj.write(buffer) 264 | file_obj.write("\n") 265 | 266 | if __name__ == '__main__': 267 | rospy.init_node('init_rovio_enu') 268 | rospy.loginfo(rospy.get_name() + " start") 269 | 270 | # go to class functions that do all the heavy lifting. Do error checking. 271 | try: 272 | init_rovio_enu = InitRovioEnu() 273 | except rospy.ROSInterruptException: pass 274 | 275 | -------------------------------------------------------------------------------- /mav_rtk_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_rtk_gui) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED 6 | COMPONENTS 7 | rospy 8 | piksi_rtk_msgs 9 | nav_msgs 10 | ) 11 | 12 | ################################### 13 | ## catkin specific configuration ## 14 | ################################### 15 | ## The catkin_package macro generates cmake config files for your package 16 | ## Declare things to be passed to dependent projects 17 | ## INCLUDE_DIRS: uncomment this if your package contains header files 18 | ## LIBRARIES: libraries you create in this project that dependent projects also need 19 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 20 | ## DEPENDS: system dependencies of this project that dependent projects also need 21 | catkin_package( 22 | # INCLUDE_DIRS 23 | # LIBRARIES 24 | CATKIN_DEPENDS 25 | piksi_rtk_msgs 26 | nav_msgs 27 | piksi_multi_rtk_ros 28 | # DEPENDS 29 | ) 30 | 31 | ############# 32 | ## Install ## 33 | ############# 34 | 35 | catkin_python_setup() 36 | 37 | catkin_install_python(PROGRAMS nodes/rtk_info nodes/rtk_npose0 nodes/rtk_npose1 38 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 39 | -------------------------------------------------------------------------------- /mav_rtk_gui/cfg/piksi_topic_names_example.yaml: -------------------------------------------------------------------------------- 1 | ## 2 | # If you put here relative topic names, they will be append to namespace. 3 | # If you put absolute topics, no leading namespace will be added. 4 | ## 5 | namespace: 'auk' 6 | 7 | # RTK Info topics 8 | piksi_receiver_state_topic: 'piksi/debug/receiver_state' 9 | piksi_uart_state_topic: 'piksi/debug/uart_state' 10 | piksi_baseline_ned_topic: 'piksi/baseline_ned' 11 | piksi_age_of_corrections_topic: 'piksi/age_of_corrections' 12 | 13 | # MSF topics and services 14 | msf_odometry_topic: 'msf_core/odometry' 15 | init_msf_scale_srv: 'pose_sensor_rovio/pose_sensor/initialize_msf_scale' 16 | init_msf_height_srv: 'pose_sensor_rovio/pose_sensor/initialize_msf_height' 17 | 18 | # Init Rovio service 19 | init_rovio_srv: 'init_rovio_enu/send_reset_to_rovio' 20 | -------------------------------------------------------------------------------- /mav_rtk_gui/launch/rtk_info_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /mav_rtk_gui/launch/rtk_npose0_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mav_rtk_gui/launch/rtk_npose1_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /mav_rtk_gui/nodes/rtk_info: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rtk_info.py 5 | # Description: ROS module to create basic GUI to check RTK status. 6 | # 7 | 8 | import rospy 9 | from Tkinter import * 10 | 11 | from mav_rtk_gui.gui_frames.rtk_info_frame import RtkInfoFrame 12 | from mav_rtk_gui.gui_frames.rtk_fix_plot_frame import RtkFixPlotFrame 13 | 14 | class RtkInfo: 15 | def __init__(self): 16 | # Main tkinter window. 17 | self.root = Tk() 18 | self.root.wm_title("RTK Info") 19 | 20 | # Screen division. 21 | self.column_0 = Frame(self.root) 22 | self.column_0.pack(side = LEFT) 23 | 24 | self.row_0 = Frame(self.column_0) 25 | self.row_0.pack(side = TOP) 26 | self.row_1 = Frame(self.column_0) 27 | self.row_1.pack(side = TOP) 28 | 29 | # Create required frames. 30 | self.rtk_info_frame = self.create_rtk_info_frame(self.row_0) 31 | self.rtk_fix_plot_frame = self.create_rtk_fix_plot_frame(self.row_1) 32 | 33 | # rospy.spin is not needed in this case. 34 | self.root.mainloop() 35 | 36 | def create_rtk_info_frame(self, parent_frame): 37 | rtk_info_frame = RtkInfoFrame(parent_frame) 38 | 39 | return rtk_info_frame 40 | 41 | def create_rtk_fix_plot_frame(self, parent_frame): 42 | rtk_fix_plot_frame = RtkFixPlotFrame(parent_frame) 43 | 44 | return rtk_fix_plot_frame 45 | 46 | if __name__ == '__main__': 47 | rospy.init_node('rtk_info') 48 | rospy.loginfo(rospy.get_name() + ' start') 49 | 50 | # Go to class functions that do all the heavy lifting. Do error checking. 51 | try: 52 | rtk_info_gui = RtkInfo() 53 | except rospy.ROSInterruptException: 54 | pass 55 | -------------------------------------------------------------------------------- /mav_rtk_gui/nodes/rtk_npose0: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rtk_info.py 5 | # Description: ROS module to create a GUI to check RTK status, reset Rovio and reset MSF. 6 | # 7 | 8 | import rospy 9 | from Tkinter import * 10 | 11 | from mav_rtk_gui.gui_frames.rtk_info_frame import RtkInfoFrame 12 | from mav_rtk_gui.gui_frames.rtk_fix_plot_frame import RtkFixPlotFrame 13 | from mav_rtk_gui.gui_frames.rovio_frame import RovioFrame 14 | from mav_rtk_gui.gui_frames.msf_frame import MsfFrame 15 | 16 | class RtkNpose0: 17 | def __init__(self): 18 | # Main tkinter window. 19 | self.root = Tk() 20 | self.root.wm_title("RTK & Rovio NPOSE0 & MSF") 21 | 22 | # Screen division. 23 | self.column_0 = Frame(self.root) 24 | self.column_0.pack(side = LEFT) 25 | self.column_1 = Frame(self.root) 26 | self.column_1.pack(side = LEFT) 27 | 28 | # RTK Stuff. 29 | self.frame_0_0 = Frame(self.column_0) 30 | self.frame_0_0.pack(side = TOP) 31 | self.frame_0_1 = Frame(self.column_0) 32 | self.frame_0_1.pack(side = TOP) 33 | 34 | # MSF. 35 | self.frame_1_0 = Frame(self.column_1) 36 | self.frame_1_0.pack(side = TOP) 37 | self.frame_1_1 = Frame(self.column_1) 38 | self.frame_1_1.pack(side = TOP) 39 | 40 | # Create RTK frames. 41 | self.rtk_info_frame = self.create_rtk_info_frame(self.frame_0_0) 42 | self.rtk_fix_plot_frame = self.create_rtk_fix_plot_frame(self.frame_0_1) 43 | 44 | # Create reset rovio frame. 45 | self.reset_rovio_frame = self.create_reset_rovio_frame(self.frame_1_0) 46 | 47 | # Create MSF frame. 48 | self.msf_frame = self.create_msf_frame(self.frame_1_1) 49 | 50 | # rospy.spin is not needed in this case. 51 | self.root.mainloop() 52 | 53 | def create_rtk_info_frame(self, parent_frame): 54 | rtk_info_frame = RtkInfoFrame(parent_frame) 55 | 56 | return rtk_info_frame 57 | 58 | def create_rtk_fix_plot_frame(self, parent_frame): 59 | rtk_fix_plot_frame = RtkFixPlotFrame(parent_frame) 60 | 61 | return rtk_fix_plot_frame 62 | 63 | def create_reset_rovio_frame(self, parent_frame): 64 | reset_rovio_frame = RovioFrame(parent_frame) 65 | 66 | return reset_rovio_frame 67 | 68 | def create_msf_frame(self, parent_frame): 69 | msf_frame = MsfFrame(parent_frame) 70 | 71 | return msf_frame 72 | 73 | if __name__ == '__main__': 74 | rospy.init_node('rtk_npose0') 75 | rospy.loginfo(rospy.get_name() + ' start') 76 | 77 | # Go to class functions that do all the heavy lifting. Do error checking. 78 | try: 79 | rtk_npose0_gui = RtkNpose0() 80 | except rospy.ROSInterruptException: 81 | pass 82 | -------------------------------------------------------------------------------- /mav_rtk_gui/nodes/rtk_npose1: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rtk_info.py 5 | # Description: ROS module to create a GUI to check RTK status and reset MSF. 6 | # 7 | 8 | import rospy 9 | from Tkinter import * 10 | 11 | from mav_rtk_gui.gui_frames.rtk_info_frame import RtkInfoFrame 12 | from mav_rtk_gui.gui_frames.rtk_fix_plot_frame import RtkFixPlotFrame 13 | from mav_rtk_gui.gui_frames.msf_frame import MsfFrame 14 | 15 | class RtkNpose1: 16 | def __init__(self): 17 | # Main tkinter window. 18 | self.root = Tk() 19 | self.root.wm_title("RTK & MSF") 20 | 21 | # Screen division. 22 | self.column_0 = Frame(self.root) 23 | self.column_0.pack(side = LEFT) 24 | self.column_1 = Frame(self.root) 25 | self.column_1.pack(side = LEFT) 26 | 27 | # RTK Stuff. 28 | self.frame_0_0 = Frame(self.column_0) 29 | self.frame_0_0.pack(side = TOP) 30 | self.frame_0_1 = Frame(self.column_0) 31 | self.frame_0_1.pack(side = TOP) 32 | 33 | # MSF. 34 | self.frame_1_0 = Frame(self.column_1) 35 | self.frame_1_0.pack(side = TOP) 36 | 37 | # Create RTK frames. 38 | self.rtk_info_frame = self.create_rtk_info_frame(self.frame_0_0) 39 | self.rtk_fix_plot_frame = self.create_rtk_fix_plot_frame(self.frame_0_1) 40 | 41 | # Create MSF frame. 42 | self.msf_frame = self.create_msf_frame(self.frame_1_0) 43 | 44 | # rospy.spin is not needed in this case. 45 | self.root.mainloop() 46 | 47 | def create_rtk_info_frame(self, parent_frame): 48 | rtk_info_frame = RtkInfoFrame(parent_frame) 49 | 50 | return rtk_info_frame 51 | 52 | def create_rtk_fix_plot_frame(self, parent_frame): 53 | rtk_fix_plot_frame = RtkFixPlotFrame(parent_frame) 54 | 55 | return rtk_fix_plot_frame 56 | 57 | def create_msf_frame(self, parent_frame): 58 | msf_frame = MsfFrame(parent_frame) 59 | 60 | return msf_frame 61 | 62 | if __name__ == '__main__': 63 | rospy.init_node('rtk_npose1') 64 | rospy.loginfo(rospy.get_name() + ' start') 65 | 66 | # Go to class functions that do all the heavy lifting. Do error checking. 67 | try: 68 | rtk_npose1_gui = RtkNpose1() 69 | except rospy.ROSInterruptException: 70 | pass 71 | -------------------------------------------------------------------------------- /mav_rtk_gui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mav_rtk_gui 4 | 0.0.0 5 | 6 | GUI interfaces to check RTK status and reset state estimation. 7 | 8 | 9 | Marco Tranzatto 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | rospy 16 | piksi_rtk_msgs 17 | nav_msgs 18 | piksi_multi_rtk_ros 19 | 20 | -------------------------------------------------------------------------------- /mav_rtk_gui/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['mav_rtk_gui'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/mav_rtk_gps/94f97b24ec30c58b2632cbd4ed7becbe6a3f0b6d/mav_rtk_gui/src/mav_rtk_gui/__init__.py -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/mav_rtk_gps/94f97b24ec30c58b2632cbd4ed7becbe6a3f0b6d/mav_rtk_gui/src/mav_rtk_gui/gui_frames/__init__.py -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/helpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: helpers.py 5 | # Description: Collection of handy functions. 6 | # 7 | def get_full_namespace(name_space, topic): 8 | # check if topic is already absolute 9 | if topic[0] == '/': 10 | full_name = topic 11 | else: 12 | if name_space == '': 13 | # no namespace specified, use absolute name 14 | full_name = '/' + topic 15 | else: 16 | # add leading namespace 17 | full_name = '/' + name_space + '/' + topic 18 | 19 | return full_name -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/msf_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: msf_frame.py 5 | # Description: Reset MSF frame to be attached to a GUI. 6 | # 7 | import rospy 8 | from Tkinter import * 9 | import matplotlib 10 | import helpers 11 | 12 | matplotlib.use('TkAgg') 13 | from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2TkAgg 14 | from matplotlib.figure import Figure 15 | import sensor_fusion_comm.srv 16 | from collections import deque 17 | 18 | from nav_msgs.msg import Odometry 19 | 20 | maxLengthDequeArray = 150 21 | figureSizeWidth = 6.5 22 | figureSizeHeight = 5 23 | 24 | class MsfFrame: 25 | def __init__(self, parent_window): 26 | # Topics & services Names. 27 | (self.topic_names, self.service_names) = self.get_topic_service_names() 28 | 29 | # Other parameters. 30 | self.odometry_down_sample_factor = rospy.get_param('~msf_odometry_down_sample_factor', 10.0) 31 | 32 | self.main_label = Label(parent_window, text='MSF', font="Times 14 bold") 33 | self.main_label.grid(row=0, columnspan=2) 34 | 35 | # Initialize msf height. 36 | current_row = 1 37 | self.init_height_button = Button(parent_window, 38 | text="Init Height", 39 | command=self.init_height_handler) 40 | self.init_height_button.grid(row=current_row, column=0) 41 | self.init_height_entry = Entry(parent_window) 42 | self.init_height_entry.insert(0, '1.0') # default value 43 | self.init_height_entry.grid(row=current_row, column=1) 44 | 45 | # Initialize msf scale. 46 | current_row = 2 47 | self.init_scale_button = Button(parent_window, 48 | text="Init Scale", 49 | command=self.init_scale_handler) 50 | self.init_scale_button.grid(row=current_row, column=0) 51 | self.init_scale_entry = Entry(parent_window) 52 | self.init_scale_entry.insert(0, '1.0') # default value 53 | self.init_scale_entry.grid(row=current_row, column=1) 54 | 55 | # Reset msf view. 56 | current_row = 3 57 | self.reset_view_button = Button(parent_window, 58 | text="Reset View", 59 | command=self.reset_view_handler) 60 | self.reset_view_button.grid(row=current_row, column=0) 61 | 62 | # Local altitude. 63 | current_row = 4 64 | self.altitude_label = Label(parent_window, text="Local Altitude [m]: ", font="Sans 8") 65 | self.altitude_label.grid(row=current_row, column=0) 66 | self.altitude_status = Label(parent_window, text="", font="Sans 8") 67 | self.altitude_status.grid(row=current_row, column=1) 68 | 69 | # Subplots for position and velocity from MSF. 70 | self.figure = Figure(figsize=(figureSizeWidth, figureSizeHeight), dpi=75) 71 | self.figure.subplots_adjust(hspace=.4) 72 | 73 | self.first_odometry_received = False 74 | self.reset_plot_view = False 75 | self.first_time_odometry = 0 76 | 77 | self.canvas = FigureCanvasTkAgg(self.figure, master=parent_window) 78 | self.canvas.show() 79 | self.canvas.get_tk_widget().grid(rowspan=4, columnspan=2) 80 | 81 | # Position. 82 | self.axes_position = [] 83 | 84 | # Velocity. 85 | self.axes_velocity = [] 86 | 87 | # Position data. 88 | self.odometry_msg_count = [] 89 | self.time_odometry = [] 90 | self.x = [] 91 | self.y = [] 92 | self.z = [] 93 | self.line_x = [] 94 | self.line_y = [] 95 | self.line_z = [] 96 | 97 | # Velocity data. 98 | self.vx = [] 99 | self.vy = [] 100 | self.vz = [] 101 | self.line_vx = [] 102 | self.line_vy = [] 103 | self.line_vz = [] 104 | 105 | # Init data. 106 | self.reset_view_handler() 107 | 108 | # Make labels tight 109 | self.figure.tight_layout() 110 | 111 | # Subscribe to topics. 112 | rospy.Subscriber(self.topic_names['msf_odometry'], Odometry, 113 | self.odometry_callback) 114 | 115 | # Services. 116 | self.init_height_srv = rospy.ServiceProxy(self.service_names['init_msf_height'], 117 | sensor_fusion_comm.srv.InitHeight) 118 | self.init_scale_srv = rospy.ServiceProxy(self.service_names['init_msf_scale'], 119 | sensor_fusion_comm.srv.InitScale) 120 | 121 | def get_topic_service_names(self): 122 | topic_names = {} 123 | service_names = {} 124 | 125 | # Topics. 126 | topic_names['msf_odometry'] = rospy.get_param('~msf_odometry_topic', 'msf_core/odometry') 127 | 128 | # Services. 129 | service_names['init_msf_height'] = rospy.get_param('~init_msf_height_srv', 130 | 'pose_sensor_rovio/pose_sensor/initialize_msf_height') 131 | service_names['init_msf_scale'] = rospy.get_param('~init_msf_scale_srv', 132 | 'pose_sensor_rovio/pose_sensor/initialize_msf_scale') 133 | 134 | # Check if we should add a leading namespace 135 | name_space = rospy.get_param('~namespace', '') 136 | for key, value in topic_names.iteritems(): 137 | topic_names[key] = helpers.get_full_namespace(name_space, value) 138 | 139 | for key, value in service_names.iteritems(): 140 | service_names[key] = helpers.get_full_namespace(name_space, value) 141 | 142 | return topic_names, service_names 143 | 144 | def init_scale_handler(self): 145 | new_scale = float(self.init_scale_entry.get()) 146 | self.init_scale_srv(new_scale) 147 | 148 | def init_height_handler(self): 149 | new_height = float(self.init_height_entry.get()) 150 | self.init_height_srv(new_height) 151 | 152 | def reset_view_handler(self): 153 | 154 | if not self.first_odometry_received: 155 | # Init subplots. 156 | self.axes_position = self.figure.add_subplot(211) 157 | self.axes_velocity = self.figure.add_subplot(212) 158 | 159 | else: 160 | # Clear subplots. 161 | self.axes_position.clear() 162 | self.axes_velocity.clear() 163 | 164 | # Position. 165 | self.axes_position.set_xlabel('Time [s]') 166 | self.axes_position.set_ylabel('Position [m]') 167 | self.axes_position.grid() 168 | 169 | # Velocity. 170 | self.axes_velocity.set_xlabel('Time [s]') 171 | self.axes_velocity.set_ylabel('Velocity [m/s]') 172 | self.axes_velocity.grid() 173 | 174 | # Position data. 175 | self.odometry_msg_count = 0 176 | self.time_odometry = deque([], maxlen=maxLengthDequeArray) 177 | self.x = deque([], maxlen=maxLengthDequeArray) 178 | self.y = deque([], maxlen=maxLengthDequeArray) 179 | self.z = deque([], maxlen=maxLengthDequeArray) 180 | self.line_x = [] 181 | self.line_y = [] 182 | self.line_z = [] 183 | 184 | # Velocity data. 185 | self.vx = deque([], maxlen=maxLengthDequeArray) 186 | self.vy = deque([], maxlen=maxLengthDequeArray) 187 | self.vz = deque([], maxlen=maxLengthDequeArray) 188 | self.line_vx = [] 189 | self.line_vy = [] 190 | self.line_vz = [] 191 | 192 | self.reset_plot_view = True 193 | 194 | def odometry_callback(self, msg): 195 | # Downsample odometry. 196 | self.odometry_msg_count += 1 197 | if (self.odometry_msg_count % self.odometry_down_sample_factor) != 0: 198 | return 199 | 200 | # Data time. 201 | secs = msg.header.stamp.to_sec() 202 | 203 | if not self.first_odometry_received or self.reset_plot_view: 204 | self.first_odometry_received = True 205 | self.reset_plot_view = False 206 | self.first_time_odometry = secs 207 | 208 | # Plot empty lines for position. 209 | self.line_x, self.line_y, self.line_z = self.axes_position.plot([], [], 'r--', 210 | [], [], 'g--', 211 | [], [], 'b--') 212 | self.line_x.set_linewidth(2.0) 213 | self.line_y.set_linewidth(2.0) 214 | self.line_z.set_linewidth(2.0) 215 | 216 | # Plot empty lines for velocity. 217 | self.line_vx, self.line_vy, self.line_vz = self.axes_velocity.plot([], [], 'r--', 218 | [], [], 'g--', 219 | [], [], 'b--') 220 | 221 | self.line_vx.set_linewidth(2.0) 222 | self.line_vy.set_linewidth(2.0) 223 | self.line_vz.set_linewidth(2.0) 224 | 225 | # Local altitude. 226 | self.altitude_status['text'] = str(round(msg.pose.pose.position.z, 2)) 227 | 228 | # Position data. 229 | self.time_odometry.append(secs - self.first_time_odometry) 230 | self.x.append(msg.pose.pose.position.x) 231 | self.y.append(msg.pose.pose.position.y) 232 | self.z.append(msg.pose.pose.position.z) 233 | 234 | # Velocity data. 235 | self.vx.append(msg.twist.twist.linear.x) 236 | self.vy.append(msg.twist.twist.linear.y) 237 | self.vz.append(msg.twist.twist.linear.z) 238 | 239 | # Update position lines. 240 | self.line_x.set_xdata(self.time_odometry) 241 | self.line_x.set_ydata(self.x) 242 | self.line_y.set_xdata(self.time_odometry) 243 | self.line_y.set_ydata(self.y) 244 | self.line_z.set_xdata(self.time_odometry) 245 | self.line_z.set_ydata(self.z) 246 | 247 | # Adjust view based on new data. 248 | self.axes_position.relim() 249 | self.axes_position.autoscale_view() 250 | 251 | # Update velocity lines. 252 | self.line_vx.set_xdata(self.time_odometry) 253 | self.line_vx.set_ydata(self.vx) 254 | self.line_vy.set_xdata(self.time_odometry) 255 | self.line_vy.set_ydata(self.vy) 256 | self.line_vz.set_xdata(self.time_odometry) 257 | self.line_vz.set_ydata(self.vz) 258 | 259 | # Adjust view based on new data. 260 | self.axes_velocity.relim() 261 | self.axes_velocity.autoscale_view() 262 | 263 | # Final draw. 264 | self.canvas.draw() 265 | -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/rovio_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rovio_frame.py 5 | # Description: Reset Rovio frame to be attached to a GUI. 6 | # 7 | import rospy 8 | import helpers 9 | 10 | from Tkinter import * 11 | import std_srvs.srv 12 | 13 | class RovioFrame: 14 | def __init__(self, parent_window): 15 | # Service Names. 16 | self.service_names = self.get_service_names() 17 | 18 | self.main_label = Label(parent_window, text="Reset Rovio (NPOSE = 0)", font="Times 14 bold") 19 | self.main_label.grid(row=0, columnspan=2) 20 | 21 | # Init rovio button. 22 | current_row = 1 23 | self.init_rovio_button = Button(parent_window, 24 | text = 'Reset Rovio', 25 | command = self.init_rovio_button_handler) 26 | 27 | self.init_rovio_button.grid(row = current_row, columnspan = 2) 28 | 29 | # Init rovio service message. 30 | current_row = 1 31 | self.init_message_label = Label(parent_window, text ='', wraplength = 450) 32 | self.init_message_label.grid(row = current_row, columnspan = 2) 33 | 34 | # Init rovio service client. 35 | self.init_rovio_srv = rospy.ServiceProxy(self.service_names['init_rovio'], 36 | std_srvs.srv.Trigger) 37 | 38 | def get_service_names(self): 39 | # Rovio services. 40 | service_names = {} 41 | 42 | service_names['init_rovio'] = rospy.get_param('~init_rovio_srv', 43 | 'init_rovio_enu/send_reset_to_rovio') 44 | 45 | # Check if we should add a leading namespace 46 | name_space = rospy.get_param('~namespace', '') 47 | for key, value in service_names.iteritems(): 48 | service_names[key] = helpers.get_full_namespace(name_space, value) 49 | 50 | return service_names 51 | 52 | def init_rovio_button_handler(self): 53 | try: 54 | response = self.init_rovio_srv() 55 | self.init_message_label['text'] = response.message 56 | 57 | except rospy.ServiceException, e: 58 | message = 'Service call to reset Rovio internal state failed: %s' % e 59 | self.init_message_label['text'] = message 60 | -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/rtk_fix_plot_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rtk_fix_plot_frame.py 5 | # Description: GPS Plot frame to be attached to a GUI. 6 | # 7 | import rospy 8 | from Tkinter import * 9 | import matplotlib 10 | import helpers 11 | 12 | matplotlib.use('TkAgg') 13 | from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2TkAgg 14 | from matplotlib.figure import Figure 15 | from collections import deque 16 | from piksi_rtk_msgs.msg import ReceiverState 17 | 18 | maxLengthDequeArray = 100 19 | figureSizeWidth = 5.5 20 | figureSizeHeight = 2 21 | 22 | class RtkFixPlotFrame: 23 | def __init__(self, parent_window): 24 | # Topic Names. 25 | self.topic_names = self.get_topic_names() 26 | 27 | self.main_label = Label(parent_window, text="RTK Fix Plot", font="Times 14 bold") 28 | self.main_label.grid(row=0, columnspan=2) 29 | 30 | # Plot for RTK fix. 31 | self.first_receiver_state_received = False 32 | self.first_time_receiver_state = 0 33 | 34 | self.figure = Figure(figsize=(figureSizeWidth, figureSizeHeight), dpi=75) 35 | 36 | self.axes_rtk_fix = self.figure.add_subplot(111) 37 | self.axes_rtk_fix.set_xlabel('Time [s]') 38 | self.axes_rtk_fix.set_ylabel('RTK Fix') 39 | self.axes_rtk_fix.grid() 40 | self.figure.tight_layout() 41 | self.axes_rtk_fix.set_yticks([0.0, 1.0]) 42 | 43 | self.canvas = FigureCanvasTkAgg(self.figure, master=parent_window) 44 | self.canvas.show() 45 | self.canvas.get_tk_widget().grid(rowspan=4, columnspan=2) 46 | 47 | # Position data. 48 | self.odometry_msg_count = 0 49 | self.time_rtk_fix = deque([], maxlen=maxLengthDequeArray) 50 | self.rtk_fix = deque([], maxlen=maxLengthDequeArray) 51 | self.line_rtk_fix = [] 52 | 53 | # Subscribe to topics. 54 | rospy.Subscriber(self.topic_names['piksi_receiver_state'], ReceiverState, 55 | self.receiver_state_callback) 56 | 57 | def get_topic_names(self): 58 | # RTK info topics. 59 | topic_names = {} 60 | 61 | topic_names['piksi_receiver_state'] = rospy.get_param('~piksi_receiver_state_topic', 62 | 'piksi/debug/receiver_state') 63 | 64 | # Check if we should add a leading namespace 65 | name_space = rospy.get_param('~namespace', '') 66 | for key, value in topic_names.iteritems(): 67 | topic_names[key] = helpers.get_full_namespace(name_space, value) 68 | 69 | return topic_names 70 | 71 | def receiver_state_callback(self, msg): 72 | # Data time. 73 | secs = msg.header.stamp.to_sec() 74 | 75 | if not self.first_receiver_state_received: 76 | self.first_receiver_state_received = True 77 | self.first_time_receiver_state = secs 78 | 79 | # Plot empty line. 80 | self.line_rtk_fix, = self.axes_rtk_fix.plot([], [], 'k--*') 81 | self.line_rtk_fix.set_linewidth(2.0) 82 | 83 | # Data. 84 | self.time_rtk_fix.append(secs - self.first_time_receiver_state) 85 | if msg.rtk_mode_fix == True: 86 | self.rtk_fix.append(1.0) 87 | else: 88 | self.rtk_fix.append(0.0) 89 | 90 | # Update plot. 91 | self.line_rtk_fix.set_xdata(self.time_rtk_fix) 92 | self.line_rtk_fix.set_ydata(self.rtk_fix) 93 | 94 | self.axes_rtk_fix.set_xlim(min(self.time_rtk_fix), max(self.time_rtk_fix)) 95 | # Help line visualization. 96 | self.axes_rtk_fix.set_ylim(-0.1, 97 | 1.1) 98 | 99 | # Final draw. 100 | self.canvas.draw() 101 | -------------------------------------------------------------------------------- /mav_rtk_gui/src/mav_rtk_gui/gui_frames/rtk_info_frame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # 4 | # Title: rtk_info_frame.py 5 | # Description: GPS frame to be attached to a GUI. 6 | # 7 | import rospy 8 | import helpers 9 | from collections import deque 10 | 11 | from Tkinter import * 12 | 13 | from piksi_rtk_msgs.msg import ReceiverState_V2_2_15 14 | from piksi_rtk_msgs.msg import UartState 15 | from piksi_rtk_msgs.msg import BaselineNed 16 | from piksi_rtk_msgs.msg import InfoWifiCorrections 17 | from sensor_msgs.msg import NavSatFix, NavSatStatus 18 | from piksi_rtk_msgs.msg import AgeOfCorrections 19 | 20 | wifiCorrectionsHzAverage = 5 # Compute corrections Hz over wifiCorrectionsHzAverage seconds 21 | altitudeAverageSamples = 10 22 | 23 | class RtkInfoFrame: 24 | def __init__(self, parent_window): 25 | # Topic Names. 26 | self.topic_names = self.get_topic_names() 27 | 28 | # Create GUI. 29 | self.main_label = Label(parent_window, text='RTK Info', font="Times 14 bold") 30 | self.main_label.grid(row=0, columnspan=2) 31 | 32 | # Type of fix. 33 | current_row = 1 34 | self.type_of_fix_label = Label(parent_window, text="Type of fix: ", font="Sans 8") 35 | self.type_of_fix_label.grid(row=current_row) 36 | self.type_of_fix_status = Label(parent_window, text="", font="Sans 8") 37 | self.type_of_fix_status.grid(row=current_row, column=1) 38 | 39 | # Number of satellites. 40 | current_row = current_row + 1 41 | self.number_sat_label = Label(parent_window, text="Number of satellites: ", font="Sans 8") 42 | self.number_sat_label.grid(row=current_row) 43 | self.number_sat_status = Label(parent_window, text="", font="Sans 8") 44 | self.number_sat_status.grid(row=current_row, column=1) 45 | 46 | # Signal strength. 47 | current_row = current_row + 1 48 | self.signal_strength_label = Label(parent_window, text="Signal strength: ", font="Sans 8") 49 | self.signal_strength_label.grid(row=current_row) 50 | self.signal_strength_status = Label(parent_window, text="", wraplength=300, font="Sans 8") 51 | self.signal_strength_status.grid(row=current_row, column=1) 52 | 53 | # Uart A TX throughput. 54 | current_row = current_row + 1 55 | self.uart_a_tx_throughput_label = Label(parent_window, text="UART A TX throughput: ", font="Sans 8") 56 | self.uart_a_tx_throughput_label.grid(row=current_row) 57 | self.uart_a_tx_throughput_status = Label(parent_window, text="", font="Sans 8") 58 | self.uart_a_tx_throughput_status.grid(row=current_row, column=1) 59 | 60 | # Uart A RX throughput. 61 | current_row = current_row + 1 62 | self.uart_a_rx_throughput_label = Label(parent_window, text="UART A RX throughput: ", font="Sans 8") 63 | self.uart_a_rx_throughput_label.grid(row=current_row) 64 | self.uart_a_rx_throughput_status = Label(parent_window, text="", font="Sans 8") 65 | self.uart_a_rx_throughput_status.grid(row=current_row, column=1) 66 | 67 | # Uart A crc errors. 68 | current_row = current_row + 1 69 | self.uart_a_crc_errors_label = Label(parent_window, text="UART A crc errors: ", font="Sans 8") 70 | self.uart_a_crc_errors_label.grid(row=current_row) 71 | self.uart_a_crc_errors_status = Label(parent_window, text="", font="Sans 8") 72 | self.uart_a_crc_errors_status.grid(row=current_row, column=1) 73 | 74 | # Uart B TX throughput. 75 | current_row = current_row + 1 76 | self.uart_b_tx_throughput_label = Label(parent_window, text="UART B TX throughput: ", font="Sans 8") 77 | self.uart_b_tx_throughput_label.grid(row=current_row) 78 | self.uart_b_tx_throughput_status = Label(parent_window, text="", font="Sans 8") 79 | self.uart_b_tx_throughput_status.grid(row=current_row, column=1) 80 | 81 | # Uart B RX throughput. 82 | current_row = current_row + 1 83 | self.uart_b_rx_throughput_label = Label(parent_window, text="UART B RX throughput: ", font="Sans 8") 84 | self.uart_b_rx_throughput_label.grid(row=current_row) 85 | self.uart_b_rx_throughput_status = Label(parent_window, text="", font="Sans 8") 86 | self.uart_b_rx_throughput_status.grid(row=current_row, column=1) 87 | 88 | # Uart B crc errors. 89 | current_row = current_row + 1 90 | self.uart_b_crc_errors_label = Label(parent_window, text="UART B crc errors: ", font="Sans 8") 91 | self.uart_b_crc_errors_label.grid(row=current_row) 92 | self.uart_b_crc_errors_status = Label(parent_window, text="", font="Sans 8") 93 | self.uart_b_crc_errors_status.grid(row=current_row, column=1) 94 | 95 | # Number of satellites used for RTK fix. 96 | current_row = current_row + 1 97 | self.number_sat_rtk_label = Label(parent_window, text="Number of satellites used for RTK: ", font="Sans 8") 98 | self.number_sat_rtk_label.grid(row=current_row) 99 | self.number_sat_rtk_status = Label(parent_window, text="", font="Sans 8") 100 | self.number_sat_rtk_status.grid(row=current_row, column=1) 101 | 102 | # Baseline NED. 103 | current_row = current_row + 1 104 | self.baseline_ned_label = Label(parent_window, text="NED baseline from base station [m]: ", font="Sans 8") 105 | self.baseline_ned_label.grid(row=current_row) 106 | self.baseline_ned_status = Label(parent_window, text="", font="Sans 8") 107 | self.baseline_ned_status.grid(row=current_row, column=1) 108 | 109 | # Navsat Fix altitude. 110 | current_row = current_row + 1 111 | self.altitude_label = Label(parent_window, text="Navsat fix altitude (avg)[m]: ", font="Sans 8") 112 | self.altitude_label.grid(row=current_row) 113 | self.altitude_status = Label(parent_window, text="", font="Sans 8") 114 | self.altitude_status.grid(row=current_row, column=1) 115 | self.altitude = deque([], maxlen=altitudeAverageSamples) 116 | 117 | # Number of corrections over wifi. 118 | current_row = current_row + 1 119 | self.number_corrections_wifi_label = Label(parent_window, text="Number of corrections over wifi: ", 120 | font="Sans 8") 121 | self.number_corrections_wifi_label.grid(row=current_row) 122 | self.number_corrections_wifi_status = Label(parent_window, text="", font="Sans 8") 123 | self.number_corrections_wifi_status.grid(row=current_row, column=1) 124 | 125 | # Rate corrections over wifi. 126 | current_row = current_row + 1 127 | self.hz_corrections_wifi_label = Label(parent_window, text="Rate corrections over wifi [Hz]: ", font="Sans 8") 128 | self.hz_corrections_wifi_label.grid(row=current_row) 129 | self.hz_corrections_wifi_status = Label(parent_window, text="0", font="Sans 8") 130 | self.hz_corrections_wifi_status.grid(row=current_row, column=1) 131 | self.time_first_sample_moving_window = rospy.get_time() 132 | self.num_corrections_first_sample_moving_window = 0 133 | 134 | # Ping with base station. 135 | current_row = current_row + 1 136 | self.ping_corrections_wifi_label = Label(parent_window, text="Ping base station [ms]: ", font="Sans 8") 137 | self.ping_corrections_wifi_label.grid(row=current_row) 138 | self.ping_corrections_wifi_status = Label(parent_window, text="-1", font="Sans 8") 139 | self.ping_corrections_wifi_status.grid(row=current_row, column=1) 140 | 141 | # Age of correction. 142 | current_row = current_row + 1 143 | self.age_of_corrections_label = Label(parent_window, text="Age of corrections [deciseconds]: ", font="Sans 8") 144 | self.age_of_corrections_label.grid(row=current_row) 145 | self.age_of_corrections_status = Label(parent_window, text="-1", font="Sans 8") 146 | self.age_of_corrections_status.grid(row=current_row, column=1) 147 | 148 | # Subscribe to topics. 149 | rospy.Subscriber(self.topic_names['piksi_receiver_state'], ReceiverState_V2_2_15, 150 | self.receiver_state_callback) 151 | rospy.Subscriber(self.topic_names['piksi_uart_state'], UartState, 152 | self.uart_state_callback) 153 | rospy.Subscriber(self.topic_names['piksi_baseline_ned'], BaselineNed, 154 | self.baseline_ned_callback) 155 | rospy.Subscriber(self.topic_names['piksi_wifi_corrections'], InfoWifiCorrections, 156 | self.wifi_corrections_callback) 157 | rospy.Subscriber(self.topic_names['piksi_navsatfix_rtk_fix'], NavSatFix, 158 | self.navsatfix_rtk_fix_callback) 159 | rospy.Subscriber(self.topic_names['piksi_age_of_corrections'], AgeOfCorrections, 160 | self.age_of_corrections_callback) 161 | 162 | def get_topic_names(self): 163 | # RTK info topics 164 | topic_names = {} 165 | 166 | topic_names['piksi_receiver_state'] = rospy.get_param('~piksi_receiver_state_topic', 167 | 'piksi/debug/receiver_state') 168 | topic_names['piksi_uart_state'] = rospy.get_param('~piksi_uart_state_topic', 169 | 'piksi/debug/uart_state') 170 | topic_names['piksi_baseline_ned'] = rospy.get_param('~piksi_baseline_ned_topic', 171 | 'piksi/baseline_ned') 172 | topic_names['piksi_wifi_corrections'] = rospy.get_param('~piksi_num_wifi_corrections_topic', 173 | 'piksi/debug/wifi_corrections') 174 | topic_names['piksi_navsatfix_rtk_fix'] = rospy.get_param('~piksi_navsatfix_rtk_fix', 175 | 'piksi/navsatfix_rtk_fix') 176 | topic_names['piksi_age_of_corrections'] = rospy.get_param('~piksi_age_of_corrections_topic', 177 | 'piksi/age_of_corrections') 178 | 179 | # Check if we should add a leading namespace 180 | name_space = rospy.get_param('~namespace', '') 181 | for key, value in topic_names.iteritems(): 182 | topic_names[key] = helpers.get_full_namespace(name_space, value) 183 | 184 | return topic_names 185 | 186 | def receiver_state_callback(self, msg): 187 | # Type of fix. 188 | if msg.rtk_mode_fix == True: 189 | self.type_of_fix_status['bg'] = "green" 190 | self.type_of_fix_status['fg'] = "white" 191 | self.type_of_fix_status['text'] = "Fix" 192 | else: 193 | self.type_of_fix_status['bg'] = "red" 194 | self.type_of_fix_status['fg'] = "black" 195 | self.type_of_fix_status['text'] = "Float" 196 | 197 | # Number of satellites. 198 | self.number_sat_status['text'] = str(msg.num_sat) 199 | 200 | # Signal strength. 201 | buffer = '[' 202 | for single_signal_strength in msg.cn0: 203 | buffer += "%s, " % ord(single_signal_strength) 204 | # Remove last coma and space. 205 | buffer = buffer[:-2] 206 | buffer += ']' 207 | 208 | self.signal_strength_status['text'] = buffer 209 | 210 | def uart_state_callback(self, msg): 211 | # Uart A state. 212 | self.uart_a_tx_throughput_status['text'] = str(round(msg.uart_a_tx_throughput, 3)) 213 | self.uart_a_rx_throughput_status['text'] = str(round(msg.uart_a_rx_throughput, 3)) 214 | self.uart_a_crc_errors_status['text'] = str(msg.uart_a_crc_error_count) 215 | 216 | # Uart b throughput. 217 | self.uart_b_tx_throughput_status['text'] = str(round(msg.uart_b_tx_throughput, 3)) 218 | self.uart_b_rx_throughput_status['text'] = str(round(msg.uart_b_rx_throughput, 3)) 219 | self.uart_b_crc_errors_status['text'] = str(msg.uart_b_crc_error_count) 220 | 221 | def baseline_ned_callback(self, msg): 222 | # Number of satellites used for RTK fix. 223 | self.number_sat_rtk_status['text'] = str(msg.n_sats) 224 | 225 | # Baseline NED, from mm to m. 226 | buffer = "[%.3f, %.3f, %.3f]" % (msg.n / 1e3, msg.e / 1e3, msg.d / 1e3) 227 | self.baseline_ned_status['text'] = buffer 228 | 229 | def wifi_corrections_callback(self, msg): 230 | # Number of corrections received. 231 | self.number_corrections_wifi_status['text'] = str(msg.received_corrections) 232 | 233 | # Compute rate corrections. 234 | width_time_window = rospy.get_time() - self.time_first_sample_moving_window 235 | 236 | if width_time_window >= wifiCorrectionsHzAverage: 237 | samples_in_time_window = msg.received_corrections - self.num_corrections_first_sample_moving_window 238 | average_corrections_hz = samples_in_time_window / width_time_window 239 | self.hz_corrections_wifi_status['text'] = str(round(average_corrections_hz, 1)) 240 | 241 | # Reset and start again. 242 | self.num_corrections_first_sample_moving_window = msg.received_corrections 243 | self.time_first_sample_moving_window = rospy.get_time() 244 | 245 | # Ping base station. 246 | self.ping_corrections_wifi_status['text'] = str(round(msg.latency, 2)) 247 | 248 | def navsatfix_rtk_fix_callback(self, msg): 249 | self.altitude.append(msg.altitude) 250 | altitude_avg = sum(self.altitude) / len(self.altitude) 251 | self.altitude_status['text'] = str(round(altitude_avg, 2)) 252 | 253 | def age_of_corrections_callback(self, msg): 254 | self.age_of_corrections_status['text'] = msg.age 255 | --------------------------------------------------------------------------------