WIP: GPS Simulator

geotf/include/geotf/geodetic_converter.h

@@ -15,6 +15,7 @@
 #include <map>
 #include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 #include <ros/ros.h>
 #include <tf_conversions/tf_eigen.h>
 
@@ -136,7 +137,7 @@ class GeodeticConverter {
   boost::optional< std::pair<std::string, std::string>>  tf_mapping_;
 
   std::shared_ptr<tf::TransformListener> listener_;
-  std::shared_ptr<tf::TransformBroadcaster> broadcaster_;
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
 
 };
 }

geotf/package.xml

@@ -20,6 +20,6 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf_conversions</depend>
-  <depend>GDAL</depend>
+  <depend>gdal-bin</depend>
 
 </package>

geotf/src/geodetic_converter.cc

@@ -1,4 +1,7 @@
 #include <geotf/geodetic_converter.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/convert.h>
+#include <tf2_eigen/tf2_eigen.h>
 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<tf::TransformListener>();
-  broadcaster_ = std::make_shared<tf::TransformBroadcaster>();
+  broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 }
 
 // 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;
 }
-}
+}

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()

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"))

gps_sim/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>gps_sim</name>
+    <version>1.0.0</version>
+    <description>More realistic GPS simulation.</description>
+
+    <maintainer email="[email protected]">Michael Pantic</maintainer>
+    <maintainer email="[email protected]">Christian Lanegger</maintainer>
+
+    <license>BSD</license>
+    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>catkin_simple</buildtool_depend>
+    <buildtool_depend>dynamic_reconfigure</buildtool_depend>
+    <depend>dynamic_reconfigure</depend>
+    <depend>rospy</depend>
+    <depend>geometric_msgs</depend>
+    <depend>nav_msgs</depend>
+
+</package>

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])

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

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