├── .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 |
--------------------------------------------------------------------------------