Wave position (#255)

* initializing of queue and keeping constant number of elements after initialization ends

* debug functions

* copying queue after refresh period and fitting curve

* sine changed to cosine, dubegging methods added

* setup file for integrating wave_position with ROS

* get_position method, added test cases, needs debugging

* enable get_position only after initialization completes

* docstring

* cleaning up

* integrating wave_position with ROS

* removing time dependency during initialization

* moving wave_position settings to ROS parameters

* adjusting time_range for wave_position

* debugging, reducing order of model function, adding bounds to curve_fit

Author: danhaus

src/sailing_robot/launch/parameters/default.yaml

@@ -65,6 +65,12 @@ procedure/timeout: 15 #in second
 procedure/exploration_coefficient: 0.1 #number between 0 and 1
 
 
+#
+# Wave Position
+#
+wave_position/time_range: 2.5 # time window in seconds captured by the wave_position algorithm
+wave_position/refresh_time: 0.5 # how often the model is re-trained
+
 
 
 # Heading

src/sailing_robot/scripts/wave_position

@@ -9,8 +9,8 @@ from sailing_robot.wave_position import Wave_position
 ##								Setting 		   						  ##
 ############################################################################
 
-time_range = 3 # time window captured by the wave_position algorithm
-refresh_time = 0.5 # how often the model is re-trained
+time_range = rospy.get_param('wave_position/time_range') # time window captured by the wave_position algorithm
+refresh_time = rospy.get_param('wave_position/refresh_time') # how often the model is re-trained
 
 """
 The higher the time_range, the less the algorithm is sensitive to noise.

src/sailing_robot/src/sailing_robot/wave_position.py

@@ -29,22 +29,28 @@
 import copy
 import numpy as np
 from scipy.optimize import curve_fit
+from scipy import __version__ as scipy_version
+
 
 class Wave_position():
     def __init__(self, frequency, time_range, refresh_time=1):
         # frequency: rate of acceleration reading
-        # time range: readings aquired during this time window are used for predictions (idealy should be a few sea waves)
+        # time range: readings acquired during this time window are used for predictions
         # refresh time: how often is prediction function re-trained on the most recent data
 
+        if ((int(scipy_version.split('.')[0]) == 0) and (int(scipy_version.split('.')[1]) < 17)):
+            raise Exception("Your scipy is outdated. Minimal required version is 0.17.0. Your are currently running "+scipy_version)
+
         self.period = 1.0/frequency # period in seconds between each two data points
         # queue is updated every time update func is called.
         self.time_range = time_range # time range captured (size of window for fitting the curve)
         self.refresh_time = refresh_time # after this time new fit is performed
         self.queue = deque()
+        self.required_queue_length = frequency * time_range
         # xdata & ydata are updated in time_range periods
         self.xdata = np.array([])
         self.ydata = np.array([])
-        self.popt = np.array([])
+        self.popt = np.array([0, 0, 0])
         self.initializing = True
         self.last_refresh = 0 # time when last refresh occured
 
@@ -53,32 +59,37 @@ def update(self, data_point):
 
         self.queue.append(data_point)
         if (self.initializing):
-            now = time.time()
-            if (len(self.queue) <= 1):
-                self.last_refresh = now
-            if ((now - self.last_refresh) >= self.time_range):
+            if (len(self.queue) >= self.required_queue_length):
                 self.initializing = False
         else:
             # Delete the oldest value to keep the queue size constant.
             self.queue.popleft()
             now = time.time()
             if ((now - self.last_refresh) >= self.refresh_time):
-                self.last_refresh = now
-                # Copy current queue to self.ydata.
-                self.process_queue()
-                # Fit model_func to the data.
-                self.train()
-
-
-    def model_func(self, x, a, b, c, d):
-        return a * (np.cos(b * x + c)) + d
+                try:
+                    # Copy current queue to self.ydata.
+                    self.process_queue()
+                    # Fit model_func to the data.
+                    self.train()
+                    self.last_refresh = now
+                except RuntimeError as e:
+                    print(e) 
+
+    def model_func(self, x, a, b, c):
+        return a * (np.cos(b * x + c)) + np.mean(self.ydata)
 
     def train(self):
         """
         Fits model_func to self.xdata & self.ydata and saves fit parameters
         to self.popt.
         """
-        self.popt, pcov = curve_fit(self.model_func, self.xdata, self.ydata)
+        guess_freq = 0.5
+        guess_amplitude = 3*np.std(self.ydata)/(2**0.5)
+        guess_phase = 0
+        p0=[guess_amplitude, guess_freq, guess_phase]
+
+        popt, pcov = curve_fit(f=self.model_func, xdata=self.xdata, ydata=self.ydata, p0=p0, bounds=((0, 0, 0), (100., 2., np.pi)), maxfev=2000)
+        self.popt = popt
 
     def process_queue(self):
         """
@@ -100,12 +111,21 @@ def get_position(self):
         (It is relative distance from the last crest.)
         """
         if (not self.initializing):
-            a, b, c, d = self.popt
             pi = np.pi
+            amplitude, frequency, phase = self.popt
+            last_refresh = self.last_refresh
             now = time.time()
-            diff = float(now - self.refresh_time)
-            cos_inner = float(b * diff + c)
+            diff = float(now - last_refresh)
+            cos_inner = float(frequency * diff + phase)
             rel_distance = cos_inner/pi - int(cos_inner/pi)
+
+            # print("frequency: {}".format(frequency))
+            # print("diff: {}".format(diff))
+            # print("phase: {}".format(phase))
+            # print("cos_inner: {}".format(cos_inner))
+            # print("rel_distance: {}".format(rel_distance))
+            # print("\n")
+
             return rel_distance
         else:
             return "initializing"
@@ -121,17 +141,26 @@ def get_position(self):
     #     np.random.seed(1729)
     #     y_noise = 0.2 * np.random.normal(size=self.xdata.size)
     #     self.ydata = y + y_noise
-    #
-    # def plot(self):
+    
+    # def plot_all(self):
+    #     import matplotlib.pyplot as plt
+    #     plt.plot(self.xdata, self.ydata, 'b*', label='training data')
+    #     plt.plot(self.xdata, self.model_func(self.xdata, *self.popt), 'r+', label='predicted')
+    #     plt.legend(loc=0)
+    #     plt.show()
+
+    # def plot_training_data(self):
     #     import matplotlib.pyplot as plt
-    #     plt.plot(self.xdata, self.ydata, 'b*', label='data')
-    #     plt.plot(self.xdata, self.model_func(self.xdata, *self.popt), 'r+')
+    #     plt.plot(self.xdata, self.ydata, 'b*')
     #     plt.show()
-    #
+    
     # def print_vars(self):
     #     print("period: {}\n".format(self.period))
     #     print("time_range: {}\n".format(self.time_range))
     #     print("refresh_time: {}\n".format(self.refresh_time))
+    #     print("queue size: {}\n".format(len(self.queue)))
+    #     print("xdata size: {}\n".format(len(self.xdata)))
+    #     print("ydata size: {}\n".format(len(self.ydata)))
     #     print("queue: {}\n".format(self.queue))
     #     print("xdata: {}\n".format(self.xdata))
     #     print("ydata: {}\n".format(self.ydata))