Fixed within_vision function

added relative observations
added testing for relative observations

CoLo-AT/dataset_manager/data_generator.py

@@ -2,7 +2,6 @@ import numpy as np
 from math import pi, sqrt, atan2, hypot, sin, cos
 import matplotlib.pyplot as plt
 
-
 class DataGenerator():
     def __init__(self, boundary, landmarks, duration, robot_labels, starting_states, start_time, delta_t,
     velocity_noise = 0, angular_velocity_noise = 0, measurement_range_noise = 0, bearing_noise = 0,  communication_noise = 0, 
@@ -145,22 +144,36 @@ class DataGenerator():
                 self.odometry_data[i].append({'time': self.time_arr['odometry'][i][time_idx], 'velocity' : velocity, 'angular velocity' : angular_velocity, 
                 'orientation' : self.groundtruth_data[i][time_idx]['orientation'], 'delta_t' : self.delta_t})
 
+    # TODO - add relative robot measurements
     def generate_measurement_data(self):
          # Generate Measurement Data
         for robot_idx, label in enumerate(self.robot_labels, 0):
             # NOTE - time_arr is used to emplace a one-to-one index-to-time correspondence 
             # between measurement_data and time_arr['measurement'] to account for varing number of landmark observations
             time_arr = []
-            # iterate over all possible times and (potentially) observable landmarks
             for time_idx in range(0, len(self.time_arr['measurement'][robot_idx])):
-               for landmarkID, landmark_loc in self.landmark_map.items():
-                    
-                    time = self.time_arr['measurement'][robot_idx][time_idx]
-                    robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
-                    robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
-                    robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
-                    robot_loc = [robot_loc_x, robot_loc_y]
+                
+                time = self.time_arr['measurement'][robot_idx][time_idx]
+                robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
+                robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
+                robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
+                robot_loc = [robot_loc_x, robot_loc_y]
+
+                # Relative (other robots) observations
+                other_robot_locs  = {label : [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']] for i, label in enumerate(self.robot_labels, 0) if i != robot_idx }
+                for robotID, other_robot_loc in other_robot_locs.items():
                     
+                    if (self.within_vision(self.robot_fov, robot_loc, other_robot_loc, robot_orientation)):
+                        measurement_range = self.calc_distance(robot_loc, other_robot_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
+                        bearing = atan2((other_robot_loc[1]-robot_loc[1]), (other_robot_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise) # global coordinates
+                        bearing = bearing - robot_orientation # NOTE - local = global - orientation, all between 0 and 2*pi
+                        bearing = self.converge_to_bearing_range(bearing)
+                        self.measurement_data[robot_idx].append({'time': time, 'subject_ID': robotID, 'measurment_range':measurement_range, 'bearing':bearing})
+                        time_arr.append(time) # we expect repeats/skipped times in time array
+
+                # Landmark (absolute) observations
+                for landmarkID, landmark_loc in self.landmark_map.items():
+
                     if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, robot_orientation)):
                         measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
                         bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise) # global coordinates
@@ -217,18 +230,36 @@ class DataGenerator():
                 measurements = [measurement for measurement in measurement_list if measurement['time']==curr_time]
                 curr_loc = [x2,y2]
                 for measurement in measurements:
-                    landmark_loc = self.landmark_map[measurement['subject_ID']]
-                    
-                    actual_meas = self.calc_distance(curr_loc, landmark_loc)
-                    generated_measurement = measurement['measurment_range']
+                    # Verify a landmark measurement
+                    if (measurement['subject_ID'] > 5):
+                        landmark_loc = self.landmark_map[measurement['subject_ID']]
+                        
+                        actual_meas = self.calc_distance(curr_loc, landmark_loc)
+                        generated_measurement = measurement['measurment_range']
+
+                        actual_bearing = self.converge_to_bearing_range(atan2(landmark_loc[1]- y2, landmark_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
+                        generated_bearing = measurement['bearing']
+
+                        meas_diff = actual_meas - generated_measurement
+                        bearing_diff = actual_bearing - generated_bearing
+
+                        meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
+                    else:
+                        other_robot_loc_x = self.groundtruth_data[self.robot_labels.index(measurement['subject_ID'])][g_idx]['x_pos']
+                        other_robot_loc_y = self.groundtruth_data[self.robot_labels.index(measurement['subject_ID'])][g_idx]['y_pos']
+
+                        other_robot_loc = [other_robot_loc_x, other_robot_loc_y]
+
+                        actual_meas = self.calc_distance(curr_loc, other_robot_loc)
+                        generated_measurement = measurement['measurment_range']
 
-                    actual_bearing = self.converge_to_bearing_range(atan2(landmark_loc[1]- y2, landmark_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
-                    generated_bearing = measurement['bearing']
+                        actual_bearing = self.converge_to_bearing_range(atan2(other_robot_loc[1]- y2, other_robot_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
+                        generated_bearing = measurement['bearing']
 
-                    meas_diff = actual_meas - generated_measurement
-                    bearing_diff = actual_bearing - generated_bearing
+                        meas_diff = actual_meas - generated_measurement
+                        bearing_diff = actual_bearing - generated_bearing
 
-                    meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
+                        meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
 
         fig, ax_arr = plt.subplots(6)
         plt.xlabel('t [s]')
@@ -292,15 +323,12 @@ class DataGenerator():
         return distance
 
     # NOTE - add distance limitations to robot_vision?
-    # TODO - FIX THIS
     # Input: robot_loc, landmark_loc, [x,y]
     # Output: boolean, a given point is with the field of view (sector of a circle) of a robot
     def within_vision(self, robot_fov, robot_loc, landmark_loc, robot_orientation=0):    
-        # between pi and -pi with respect to the x axis
-        bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0]))
-        if bearing < 0:
-            bearing += 2*pi
-        return True # bearing <= robot_fov/2 + robot_orientation and bearing >= robot_orientation - robot_fov/2
+        # between pi and -pi with respect to LOCAL FRAME
+        bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0])) - robot_orientation
+        return abs(bearing) <= robot_fov/2
 
     # get angle between -pi and pi
     def converge_to_bearing_range(self, theta):

CoLo-AT/dataset_manager/simulated_dataset_manager.py

@@ -13,9 +13,8 @@ from dataset_manager.data_generator import DataGenerator
 
 
 # TODO 
-# - Circular path data in DataGenerator class
-# - communication
-# - relative observations
+# communication
+# scheduling
 class SimulatedDataSetManager:
 
     def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,