Update to use sgptools 1.0.6

launch/params/apm_config.yaml

@@ -23,7 +23,7 @@
     time_ref_source: "fcu"  # time_reference source
     timesync_mode: MAVLINK
     timesync_avg_alpha: 0.6 # timesync averaging factor
-    timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
+    timesync_rate: 0.1    # TIMESYNC rate in Hertz (feature disabled if 0.0)
     system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
 
 # --- mavros plugins (alphabetical order) ---

requirements.txt

@@ -1,3 +1,4 @@
 sgptools
 geopandas
-pygeodesy
+pygeodesy
+shapely

scripts/offline_ipp.py

@@ -1,11 +1,12 @@
 #! /usr/bin/env python3
 
 import os
-from utils import plan2data, project_waypoints
+from utils import plan2data
 from ament_index_python.packages import get_package_share_directory
 
 import gpflow
 import numpy as np
+from sgptools.utils.misc import project_waypoints
 from sgptools.utils.tsp import run_tsp
 from sgptools.models.continuous_sgp import *
 from sgptools.models.core.transformations import *
@@ -100,10 +101,6 @@ def compute_init_paths(self):
         kernel = gpflow.kernels.RBF(lengthscales=0.1, 
                                     variance=0.5)
 
-        # Get the initial IPP solution
-        transform = IPPTransform(n_dim=2, 
-                                 sampling_rate=self.sampling_rate,
-                                 num_robots=self.num_robots)
         # Sample uniform random initial waypoints and compute initial paths
         Xu_init = get_inducing_pts(self.X_train, self.num_waypoints*self.num_robots)
 
@@ -118,29 +115,29 @@ def compute_init_paths(self):
         Xu_fixed = np.copy(Xu_init[:, :1, :])
         Xu_init = np.array(Xu_init).reshape(-1, 2)
 
-        # Initialize the SGP
+        # Get the initial IPP solution
+        transform = IPPTransform(n_dim=2, 
+                                 sampling_rate=self.sampling_rate,
+                                 num_robots=self.num_robots)
+        transform.update_Xu_fixed(Xu_fixed)
+
+        # Initialize the SGP method and optimize the path
         IPP_model, _ = continuous_sgp(self.num_waypoints, 
                                       self.X_train,
                                       likelihood_variance,
                                       kernel,
                                       transform,
                                       Xu_init=Xu_init,
-                                      max_steps=0)
-        IPP_model.transform.update_Xu_fixed(Xu_fixed)
-
-        # Get the new inducing points for the path
-        optimize_model(IPP_model, 
-                       kernel_grad=False, 
-                       optimizer='scipy',
-                       method='CG')
+                                      optimizer='scipy',
+                                      method='CG')
 
-        # Generate new paths from optimized waypoints and project them back to the bounds of the environment
+        # Project the waypoints to be within the bounds of the environment
         self.waypoints = IPP_model.inducing_variable.Z.numpy().reshape(self.num_robots, 
                                                                        self.num_waypoints, -1)
         for i in range(self.num_robots):
             self.waypoints[i] = project_waypoints(self.waypoints[i], self.X_train)
 
-        # Upscale sampling along waypoints to get a contiguous train set for each robot
+        # Upsample the path waypoints and partition the environment into seperate monitoring regions
         IPP_model.transform.sampling_rate = 30
         train_set_waypoints = IPP_model.transform.expand(IPP_model.inducing_variable.Z)
         train_set_waypoints = train_set_waypoints.numpy().reshape(self.num_robots, -1, 2)
@@ -160,7 +157,7 @@ def compute_init_paths(self):
         self.plot_paths()
 
     '''
-    Generate unlabeled training data for each robot
+    Generate unlabeled training points, i.e., monitoring regions for each robot
     '''
     def get_training_sets(self, waypoints):
         # Setup KNN training dataset

scripts/online_ipp.py

@@ -5,10 +5,10 @@
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 
 import matplotlib.pyplot as plt
-from utils import project_waypoints
 
 import gpflow
 import numpy as np
+from sgptools.utils.misc import project_waypoints
 from sgptools.models.continuous_sgp import *
 from sgptools.models.core.transformations import *
 from sgptools.models.core.osgpr import *
@@ -108,7 +108,7 @@ def __init__(self):
             sensor_subscribers.append(data_obj.get_subscriber(self))
 
         self.time_sync = ApproximateTimeSynchronizer([*sensor_subscribers],
-                                                     queue_size=10, slop=0.05)
+                                                     queue_size=10, slop=0.1)
         self.time_sync.registerCallback(self.data_callback)
 
         # Setup the timer to update the parameters and waypoints
@@ -161,6 +161,7 @@ def init_sgp_models(self):
         kernel = gpflow.kernels.RBF(lengthscales=lengthscales, 
                                     variance=variance)
         self.transform = IPPTransform(n_dim=self.n_dim,
+                                      sampling_rate=self.sampling_rate,
                                       num_robots=1)
         self.IPP_model, _ = continuous_sgp(self.num_waypoints, 
                                            self.X_train,
@@ -239,9 +240,8 @@ def plot_paths(self, waypoints):
                    delimiter=',')
 
     def update_with_data(self, force_update=False):
-        # Update the parameters and waypoints if the buffer is full and 
-        # empty the buffer after updating 
-
+        # Update the hyperparameters and waypoints if the buffer is full 
+        # or if force_update is True and atleast num_param_inducing data points are available
         if len(self.data_X) > self.buffer_size or \
             (force_update and len(self.data_X) > self.num_param_inducing):
 
@@ -287,7 +287,8 @@ def update_waypoints(self, current_waypoint):
                        method='CG')
 
         self.waypoints = self.IPP_model.inducing_variable.Z
-        self.waypoints = self.IPP_model.transform.expand(self.waypoints).numpy()
+        self.waypoints = self.IPP_model.transform.expand(self.waypoints,
+                                                         expand_sensor_model=False).numpy()
         self.waypoints = project_waypoints(self.waypoints, self.X_train)
 
     def update_param(self, X_new, y_new):
@@ -298,7 +299,7 @@ def update_param(self, X_new, y_new):
                        trainable_variables=self.param_model.trainable_variables[1:], 
                        optimizer='scipy')
 
-        self.get_logger().info(f'SSGP Kernel lengthscales: {self.param_model.kernel.lengthscales.numpy():.4f}')
+        self.get_logger().info(f'SSGP Kernel lengthscales: {self.param_model.kernel.lengthscales.numpy():.8f}')
         self.get_logger().info(f'SSGP Kernel variance: {self.param_model.kernel.variance.numpy():.4f}')
 
 

scripts/utils.py

@@ -2,7 +2,6 @@
 import numpy as np
 import geopandas as gpd
 from shapely import geometry
-from sgptools.utils.misc import *
 
 
 # Extract geofence and home location from QGC plan file
@@ -18,19 +17,4 @@ def plan2data(fname, num_samples=5000):
                                        rng=2024)
     candidates = candidates.get_coordinates().to_numpy()
 
-    return candidates, home_position
-
-# Reorder the waypoints to match the order of the points in the path
-# The waypoints are mathched to the closest points in the path 
-def reoder_path(path, waypoints):
-    dists = pairwise_distances(path, Y=waypoints, metric='euclidean')
-    _, col_ind = linear_sum_assignment(dists)
-    Xu = waypoints[col_ind].copy()
-    return Xu
-
-# Project the waypoints back to the candidate set while retaining the 
-# waypoint visitation order
-def project_waypoints(waypoints, candidates):
-    waypoints_disc = cont2disc(waypoints, candidates)
-    waypoints_valid = reoder_path(waypoints, waypoints_disc)
-    return waypoints_valid
+    return candidates, home_position