diff --git a/geotf/include/geotf/geodetic_converter.h b/geotf/include/geotf/geodetic_converter.h index f785e56..0d84b51 100644 --- a/geotf/include/geotf/geodetic_converter.h +++ b/geotf/include/geotf/geodetic_converter.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -136,7 +137,7 @@ class GeodeticConverter { boost::optional< std::pair> tf_mapping_; std::shared_ptr listener_; - std::shared_ptr broadcaster_; + std::shared_ptr broadcaster_; }; } diff --git a/geotf/package.xml b/geotf/package.xml index 6aa1fbd..9e0dc12 100644 --- a/geotf/package.xml +++ b/geotf/package.xml @@ -20,6 +20,6 @@ sensor_msgs geometry_msgs tf_conversions - GDAL + gdal-bin diff --git a/geotf/src/geodetic_converter.cc b/geotf/src/geodetic_converter.cc index 9927e77..74c42d1 100644 --- a/geotf/src/geodetic_converter.cc +++ b/geotf/src/geodetic_converter.cc @@ -1,4 +1,7 @@ #include +#include +#include +#include namespace geotf { // Initialize frame definitions from rosparams void GeodeticConverter::initFromRosParam(const std::string& prefix) { @@ -79,7 +82,7 @@ void GeodeticConverter::initFromRosParam(const std::string& prefix) { } listener_ = std::make_shared(); - broadcaster_ = std::make_shared(); + broadcaster_ = std::make_shared(); } // Adds a coordinate frame by its EPSG identifier @@ -376,11 +379,11 @@ bool GeodeticConverter::publishAsTf(const std::string& geo_input_frame, return false; } - tf::StampedTransform tf_input; - tf::transformEigenToTF(input_connection, tf_input); - tf_input.stamp_ = ros::Time::now(); - tf_input.frame_id_ = tf_connection_frame; - tf_input.child_frame_id_ = frame_name; + geometry_msgs::TransformStamped tf_input; + tf_input = tf2::eigenToTransform(input_connection); + tf_input.header.stamp = ros::Time::now(); + tf_input.header.frame_id = tf_connection_frame; + tf_input.child_frame_id = frame_name; broadcaster_->sendTransform(tf_input); return true; } @@ -470,4 +473,4 @@ bool GeodeticConverter::checkTransform(const std::string& input_frame, transforms_.insert(std::make_pair(tf_id, transform)); return true; } -} \ No newline at end of file +} diff --git a/gps_sim/CMakeLists.txt b/gps_sim/CMakeLists.txt new file mode 100644 index 0000000..cc39b87 --- /dev/null +++ b/gps_sim/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gps_sim) + +find_package(catkin REQUIRED) + +catkin_python_setup() \ No newline at end of file diff --git a/gps_sim/cfg/gps_sim.cfg b/gps_sim/cfg/gps_sim.cfg new file mode 100644 index 0000000..9a044c8 --- /dev/null +++ b/gps_sim/cfg/gps_sim.cfg @@ -0,0 +1,17 @@ +#!/usr/bin/env python +PACKAGE = "gps_sim" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +mode_enum = gen.enum([ gen.const("Auto", str_t, "auto", "Automatic selection"), + gen.const("RTK", str_t, "rtk", "RTK mode"), + gen.const("FLOAT", str_t, "float", "FLOAT mode"), + gen.const("SPP", str_t, "spp", "SPP mode"), + gen.const("NONE", str_t, "none", "GPS outage"),], + "An enum to set simulation mode") + +gen.add("sim_mode", str_t, 0, "Simulation Mode", "rtk", edit_method = mode_enum) + +exit(gen.generate(PACKAGE, "gps_sim_node", "GpsSim")) \ No newline at end of file diff --git a/gps_sim/package.xml b/gps_sim/package.xml new file mode 100644 index 0000000..708eee6 --- /dev/null +++ b/gps_sim/package.xml @@ -0,0 +1,19 @@ + + + gps_sim + 1.0.0 + More realistic GPS simulation. + + Michael Pantic + Christian Lanegger + + BSD + catkin + catkin_simple + dynamic_reconfigure + dynamic_reconfigure + rospy + geometric_msgs + nav_msgs + + diff --git a/gps_sim/python/gps_sim/GpsNoiser.py b/gps_sim/python/gps_sim/GpsNoiser.py new file mode 100644 index 0000000..fd7c317 --- /dev/null +++ b/gps_sim/python/gps_sim/GpsNoiser.py @@ -0,0 +1,44 @@ +import colorednoise as cn +import numpy as np + +class GpsNoiser: + # multiplier for each of the noise types, each of the axes + def __init__(self, white=np.array([0.1, 0.1, 0.1]), + pink=np.array([0.05, 0.05, 0.6]), + brown=np.array([0.5, 0.5, 0.1]), epsilons=np.array([0, 1, 2.5])): + self._white = white + self._pink = pink + self._brown = brown + self._sample_buffer_length = 7200 # [s] + self._sampling_freq = 10 # [hz] + # order: white, pink, brown + self._sample_buffer = np.zeros([self._sample_buffer_length * self._sampling_freq, 9]) + self._sample_buffer_pos = 0 + self._epsilons = epsilons + + self.sample_noise() + + def sample_noise(self): + # make some noise! + n = self._sample_buffer_length * self._sampling_freq + for i in range(0, 3): + self._sample_buffer[:, i] = cn.powerlaw_psd_gaussian(self._epsilons[0], n) # white + self._sample_buffer[:, i + 3] = cn.powerlaw_psd_gaussian(self._epsilons[1], n) # pink + + # for larger epsilons a low cut off frequency (fmin) has improved ease of tuning: + self._sample_buffer[:, i + 6] = cn.powerlaw_psd_gaussian(self._epsilons[2], n, fmin=0.001) # brown + + self._sample_buffer_pos = 0 + + def perturb(self, pos_enu): + if self._sample_buffer_pos >= self._sample_buffer_length * self._sampling_freq: + print("Run out of sampling buffer") + # redo -> might cause discontinuity in noise, should be avoided + self.sample_noise() + + i = self._sample_buffer_pos + self._sample_buffer_pos += 1 + + return pos_enu + np.multiply(self._white, self._sample_buffer[i, 0:3]) + \ + np.multiply(self._pink, self._sample_buffer[i, 3:6]) + \ + np.multiply(self._brown, self._sample_buffer[i, 6:9]) diff --git a/gps_sim/python/gps_sim/GpsSimulator.py b/gps_sim/python/gps_sim/GpsSimulator.py new file mode 100644 index 0000000..87ab572 --- /dev/null +++ b/gps_sim/python/gps_sim/GpsSimulator.py @@ -0,0 +1,96 @@ +import numpy as np +from gps_sim.GpsNoiser import GpsNoiser +from gps_sim.Magnetometer import Magnetometer + + +class GpsSimulator: + + def __init__(self): + self._rtk_noiser = GpsNoiser(white=np.array([0.0005, 0.0005, 0.002]), + pink=np.array([0.0035, 0.0035, 0.007]), + brown=np.array([0.001, 0.001, 0.002]), + epsilons=np.array([0, 1, 2])) + + self._float_noiser = GpsNoiser(white=np.array([0.0075, 0.0075, 0.015]), + pink=np.array([0.001, 0.001, 0.002]), + brown=np.array([0.03, 0.03, 0.06]), + epsilons=np.array([0, 1, 3.0])) + + self._spp_noiser = GpsNoiser(white=np.array([0.001, 0.001, 0.002]), + pink=np.array([0.0, 0.0, 0.0]), + brown=np.array([0.50, 0.5, 1.0]), + epsilons=np.array([0, 1, 3.0])) + + self._spp_output_cov = np.array([[1.15363866, 0.00769917, -0.02256928], + [0.00769917, 1.16177632, -0.03191735], + [-0.02256928, -0.03191735, 5.23445582]]) + + # empirical covariances from imagined model not based on data + # -> to be based on data at a later point. + self._float_output_cov = np.array([[8.17408501e-04, 7.13988338e-05, 1.47721410e-04], + [7.13988338e-05, 1.01484400e-03, -1.63778765e-04], + [1.47721410e-04, -1.63778765e-04, 2.07055086e-03]]) + + self._rtk_output_cov = np.array([[9.47897207e-05, 1.92104152e-05, -6.46848160e-05], + [1.92104152e-05, 9.39087989e-05, -7.25870764e-05], + [-6.46848160e-05, -7.25870764e-05, 4.24079132e-04]]) + + self._mag_noiser = Magnetometer() + + # can be: auto, none, spp, float, rtk + self._current_mode = "spp" + self._valid_modes = ["auto", "none", "spp", "float", "rtk"] + + self._rtk_polygon = [] + self._spp_polygon = [] + self._float_polygon = [] + self._none_polygon = [] + + def set_mode(self, mode): + if mode not in self._valid_modes: + print("Tried to set invalid mode: " + mode) + print("Resetting to auto") + self._current_mode = "auto" + else: + self._current_mode = mode + + def get_mag(self, input_rot): + return self._mag_noiser.get_field(input_rot) + + def get_gps(self, input_enu, fixtype): + if fixtype == "none": + # *screaming" lost fix!! + return None, None + + if fixtype == "rtk": + output_enu = self._rtk_noiser.perturb(input_enu) + output_cov = self._rtk_output_cov + return output_enu, output_cov + elif fixtype == "float": + output_enu = self._float_noiser.perturb(input_enu) + output_cov = self._float_output_cov + return output_enu, output_cov + elif fixtype == "spp": + output_enu = self._spp_noiser.perturb(input_enu) + output_cov = self._spp_output_cov + return output_enu, output_cov + else: + return None, None + + def get_fixtype(self, input_enu): + return "rtk" # for now + + def simulate(self, input_pose): + input_enu = input_pose[0:3, 3] + + # output GPS + if self._current_mode == "auto": + fixtype = self.get_fixtype(input_enu) + output_enu, output_cov = self.get_gps(input_enu, fixtype) + else: + output_enu, output_cov = self.get_gps(input_enu, self._current_mode) + + output_mag = self.get_mag(input_pose[0:3, 0:3]) + + # weird pythonic flex that these variables are found + return output_enu, output_cov, output_mag diff --git a/gps_sim/python/gps_sim/Magnetometer.py b/gps_sim/python/gps_sim/Magnetometer.py new file mode 100644 index 0000000..48f789a --- /dev/null +++ b/gps_sim/python/gps_sim/Magnetometer.py @@ -0,0 +1,26 @@ +import geomag as gm +import numpy as np + +import matplotlib +import matplotlib.pyplot as plt + + +class Magnetometer: + def __init__(self, noise_cov=500): + # set initial field strength + magnetic_field = gm.geomag.GeoMag() + magnetic_data = magnetic_field.GeoMag(47.37, 8.53) + self._field_vector = np.array([magnetic_data.bx, magnetic_data.by, magnetic_data.bz]) + # d = + self._noise_cov = noise_cov + self._declination = magnetic_data.dec + + # takes current orientation and rotates magnetic field according to this + # currently does not include non-linearities in calibration + def get_field(self, orientation=np.eye(3)): + noised_vector = self._field_vector + np.random.normal([0, 0, 0], + [self._noise_cov, self._noise_cov, self._noise_cov]) + return orientation.dot(noised_vector) + + def get_declination(self): + return self._declination diff --git a/gps_sim/python/gps_sim/__init__.py b/gps_sim/python/gps_sim/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/gps_sim/scripts/gps_sim_node.py b/gps_sim/scripts/gps_sim_node.py new file mode 100755 index 0000000..be4ba84 --- /dev/null +++ b/gps_sim/scripts/gps_sim_node.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python +import rospy +import numpy as np +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PointStamped +from sensor_msgs.msg import MagneticField +from geometry_msgs.msg import TransformStamped +from gps_sim.GpsSimulator import GpsSimulator +from tf import transformations + +from dynamic_reconfigure.server import Server +from gps_sim.cfg import GpsSimConfig + + +class GpsSimNode: + def __init__(self): + rospy.init_node('gps_sim_node', anonymous=True) + + self._gps_sim = GpsSimulator() + + # odom caches + self._odom_received = False + self._input_pose = [] + self._input_stamp = [] + self._input_q = [] + self._switched = False + + self._cov_orientation = np.diag([0.007607716, 0.007607716, 0.007607716]) + + # Ros stuff + self._dyn_rec_srv = Server(GpsSimConfig, self.config_callback) + self._odom_pub = rospy.Publisher('gps_odom_out', Odometry, queue_size=1) + self._pos_pub = rospy.Publisher('/stork/gps_sim/pos_spp', PointStamped, queue_size=100) + self._tf_pub = rospy.Publisher("gps_transform_out", TransformStamped, queue_size=1) + self._mag_pub = rospy.Publisher('mag_out', MagneticField, queue_size=1) + rospy.Subscriber("odom_in", Odometry, self.odom_callback) + rospy.Subscriber("/stork/leica/position", PointStamped, self.pos_callback, queue_size=100) + + def config_callback(self, cfg, lvl): + rospy.logwarn("Changing to " + "spp") + self._gps_sim.set_mode("spp") + return cfg + + def publish_at_rate(self): + if not self._odom_received: + rospy.logwarn("No odometry received") + return + + self._odom_received = False # mark data as consumed + # to prevent republishing empty data... + + # get noised data + enu_pos, enu_cov, mag_data = self._gps_sim.simulate(self._input_pose) + + # assemble messages. + mag_msg = MagneticField() + mag_msg.header.stamp = self._input_stamp + mag_msg.magnetic_field.x = mag_data[0] + mag_msg.magnetic_field.y = mag_data[1] + mag_msg.magnetic_field.z = mag_data[2] + # print(np.arctan2(mag_data[1], mag_data[0]) * (180 / 3.1415)) + self._mag_pub.publish(mag_msg) + + if enu_pos is None or enu_cov is None: + # no gps data :( + return + + odom_msg = Odometry() + odom_msg.header.stamp = self._input_stamp + odom_msg.header.frame_id = "enu" + odom_msg.pose.pose.position.x = enu_pos[0] + odom_msg.pose.pose.position.y = enu_pos[1] + odom_msg.pose.pose.position.z = enu_pos[2] + + set_orientation = True + if set_orientation: + # only get heading + rpy = transformations.euler_from_quaternion(self._input_q) + heading = rpy[2] + + # add some noise + heading_noise = heading + np.random.normal(0, np.sqrt(self._cov_orientation[2, 2]), 1) + q_head = transformations.quaternion_from_euler(0, 0, heading, 'rxyz') + + odom_msg.pose.pose.orientation.x = q_head[0] + odom_msg.pose.pose.orientation.y = q_head[1] + odom_msg.pose.pose.orientation.z = q_head[2] + odom_msg.pose.pose.orientation.w = q_head[3] + else: + odom_msg.pose.pose.orientation.w = 1.0 + + enu_cov_66 = np.eye(6) * 0.1 + enu_cov_66[0:3, 0:3] = enu_cov + enu_cov_66[3:6, 3:6] = self._cov_orientation + odom_msg.pose.covariance = enu_cov_66.flatten("C") + self._odom_pub.publish(odom_msg) + + pos_msg = PointStamped() + pos_msg.header.stamp = self._input_stamp + pos_msg.header.frame_id = "spp" + pos_msg.point.x = enu_pos[0] + pos_msg.point.y = enu_pos[1] + pos_msg.point.z = enu_pos[2] + self._pos_pub.publish(pos_msg) + + # also output as transform stamped + tf_msg = TransformStamped() + tf_msg.header.stamp = self._input_stamp + tf_msg.transform.translation.x = enu_pos[0] + tf_msg.transform.translation.y = enu_pos[1] + tf_msg.transform.translation.z = enu_pos[2] + tf_msg.transform.rotation.x = 0 + tf_msg.transform.rotation.y = 0 + tf_msg.transform.rotation.z = 0 + tf_msg.transform.rotation.w = 1 + self._tf_pub.publish(tf_msg) + + def pos_callback(self, data): + rospy.logdebug("Position received") + self._odom_received = True + # extract pose + input_pos = np.array([data.point.x, data.point.y, data.point.z]) + self._input_pose = transformations.quaternion_matrix(np.array([0.0, 0.0, 0.0, 1.0])) + + self._input_q = np.array([0.0, 0.0, 0.0, 1.0]) + + # assemble 4x4 matrix + self._input_pose[0:3, 3] = input_pos + self._input_stamp = data.header.stamp + self.publish_at_rate() + + def odom_callback(self, data): + rospy.logdebug("Odometry received") + + # if data.header.stamp.secs >= 1613926010 and data.header.stamp.secs < 1613926011 and self._gps_sim._current_mode == "rtk": + # rospy.logwarn("SWITCHED TO SPP") + # self._gps_sim.set_mode("float") + + # if data.header.stamp.secs >= 1613926010 + 30 and self._gps_sim._current_mode == "float": + # rospy.logwarn("SWITCHED TO RTK") + # self._gps_sim.set_mode("rtk") + + self._odom_received = True + # extract pose + input_pos = np.array([data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z]) + self._input_pose = transformations.quaternion_matrix(np.array([ + data.pose.pose.orientation.x, + data.pose.pose.orientation.y, + data.pose.pose.orientation.z, + data.pose.pose.orientation.w + ])) + + self._input_q = np.array([ + data.pose.pose.orientation.x, + data.pose.pose.orientation.y, + data.pose.pose.orientation.z, + data.pose.pose.orientation.w + ]) + + # assemble 4x4 matrix + self._input_pose[0:3, 3] = input_pos + self._input_stamp = data.header.stamp + + def run(self): + rospy.spin() + #r = rospy.Rate(100) # 10hz + #while not rospy.is_shutdown(): + # self.publish_at_rate() + # r.sleep() + + +if __name__ == '__main__': + node = GpsSimNode() + node.run() diff --git a/gps_sim/scripts/plot_tets.py b/gps_sim/scripts/plot_tets.py new file mode 100644 index 0000000..455f77f --- /dev/null +++ b/gps_sim/scripts/plot_tets.py @@ -0,0 +1,62 @@ + +if __name__ == "__main__": + # RTK: + # a = GpsNoiser(white=np.array([0.0005, 0.0005, 0.002]), + # pink=np.array([0.0035, 0.0035, 0.007]), + # brown=np.array([0.001, 0.001, 0.002]), + # epsilons=np.array([0, 1, 2])) + + # SPP: + a = GpsNoiser(white=np.array([0.0075, 0.0075, 0.015]), + pink=np.array([0.001, 0.001, 0.002]), + brown=np.array([0.03, 0.03, 0.06]), + epsilons=np.array([0, 1, 3.0])) + + a.sample_noise() + + # load true rtk data. + name = "FLOAT" + rtk_data = np.load("/home/mpantic/Work/ARMA/GPS/bags/bag_sim_gps/spp_2021-02-18-16-31-50.bag_np.npy") + # rtk_data = np.load("/home/mpantic/Work/ARMA/GPS/bags/bag_sim_gps/rtk_2021-02-18-16-21-18.bag_np.npy") + rtk_data -= rtk_data.mean(axis=0) + + # generate fake data of same length + sim_data = np.zeros(rtk_data.shape) + for i in range(0, len(rtk_data)): + sim_data[i, :] = a.perturb(np.array([0.0, 0.0, 0.0])) + + print("empirical covariance") + print(np.cov(sim_data.T)) + + exit(0) + + wa = 2 # working axis + s_true, f_true = mlab.psd(rtk_data[:, wa], NFFT=2 ** 12) + s_sim, f_sim = mlab.psd(sim_data[:, wa], NFFT=2 ** 12) + + plt.figure(figsize=[6, 6]) + plt.loglog(f_true, s_true, 'r', alpha=0.25) + plt.loglog(f_sim, s_sim, 'b', alpha=0.25) + plt.xlim([10 ** -3.5, 1]) + plt.ylim([10 ** -9, 10 ** 3]) + plt.title("Noise Power Spectrum " + name + " (blue=sim/red=real data)") + plt.xlabel("Freq") + plt.ylabel("Amp") + plt.grid(True) + plt.savefig("power_spec_" + name + ".png") + + plt.figure(figsize=[6, 6]) + plt.scatter(rtk_data[:, 0], rtk_data[:, 2], c=np.arange(len(rtk_data)), marker="x") + plt.title("Bias Free " + name + " ENU Position (REAL DATA)") + plt.xlabel("x_enu [m]") + plt.ylabel("y_enu [m]") + plt.grid(True) + plt.savefig("enu_xy_" + name + "_real.png") + + plt.figure(figsize=[6, 6]) + plt.scatter(sim_data[:, 0], sim_data[:, 2], c=np.arange(len(sim_data)), marker="x") + plt.title("Bias Free " + name + " ENU Position (SIM DATA)") + plt.xlabel("x_enu [m]") + plt.ylabel("y_enu [m]") + plt.grid(True) + plt.savefig("enu_xy_" + name + "_sim.png") \ No newline at end of file diff --git a/gps_sim/setup.py b/gps_sim/setup.py new file mode 100755 index 0000000..eae65e8 --- /dev/null +++ b/gps_sim/setup.py @@ -0,0 +1,12 @@ + +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['gps_sim'], + package_dir={'':'python'}) + +setup(**setup_args)